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 <memory>
35 #include <cstdlib>
36 #include <cmath>
37 #include <string>
38 #include <cstdio>
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 
48 /** @class HokuyoUrgGbxAcquisitionThread "urg_gbx_aqt.h"
49  * Laser acqusition thread for Hokuyo URG laser range finders.
50  * This thread fetches the data from the laser. This implementation uses
51  * the Gearbox library.
52  * @author Tim Niemueller
53  */
54 
55 
56 /** Constructor.
57  * @param cfg_name short name of configuration group
58  * @param cfg_prefix configuration path prefix
59  */
61  std::string &cfg_prefix)
62  : LaserAcquisitionThread("HokuyoUrgGbxAcquisitionThread")
63 {
64  set_name("HokuyoURG_GBX(%s)", cfg_name.c_str());
65  __pre_init_done = false;
66  __cfg_name = cfg_name;
67  __cfg_prefix = cfg_prefix;
68 }
69 
70 
71 void
74 {
75  if (__pre_init_done) return;
76 
77  __number_of_values = _distances_size = 360;
78 
79  __pre_init_done = true;
80 }
81 
82 void
84 {
86 
87  __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
88 
89 #ifdef HAVE_URG_GBX_9_11
90  __laser = new HokuyoLaser();
91 #if __cplusplus >= 201103L
92  std::unique_ptr<HokuyoLaser> laser(__laser);
93 #else
94  std::auto_ptr<HokuyoLaser> laser(__laser);
95 #endif
96 #else
97  __laser = new Sensor();
98 #if __cplusplus >= 201103L
99  std::unique_ptr<Sensor> laser(__laser);
100 #else
101  std::auto_ptr<Sensor> laser(__laser);
102 #endif
103 #endif
104  std::string port_options = "type=serial,device=" + __cfg_device + ",timeout=1";
105  try {
106 #ifdef HAVE_URG_GBX_9_11
107  __laser->Open(port_options);
108 #else
109  __laser->open(port_options);
110 #endif
111  } catch (flexiport::PortException &e) {
112  throw Exception("Connecting to URG laser failed: %s", e.what());
113  }
114 
115 #ifdef HAVE_URG_GBX_9_11
116  HokuyoSensorInfo info;
117  __laser->GetSensorInfo(&info);
118 
119  __data = new HokuyoData();
120  __first_ray = info.firstStep;
121  __last_ray = info.lastStep;
122  __front_ray = info.frontStep;
123 
124 #else
125  SensorInfo info;
126  __laser->get_sensor_info(info);
127  __data = new ScanData();
128 
129  __first_ray = info.first_step;
130  __last_ray = info.last_step;
131  __front_ray = info.front_step;
132 #endif
133 
134  __slit_division = info.steps;
135  __num_rays = __last_ray - __first_ray;
136  __front_idx = __front_ray - __first_ray;
137 
138  __step_per_angle = __slit_division / 360.;
139  __angle_per_step = 360. / __slit_division;
140  __angular_range = (__last_ray - __first_ray) * __angle_per_step;
141 
142  logger->log_info(name(), "VEND: %s", info.vendor.c_str());
143  logger->log_info(name(), "PROD: %s", info.product.c_str());
144  logger->log_info(name(), "FIRM: %s", info.firmware.c_str());
145  logger->log_info(name(), "PROT: %s", info.protocol.c_str());
146  logger->log_info(name(), "SERI: %s", info.serial.c_str());
147  logger->log_info(name(), "Rays range: %u..%u, front at %u (idx %u), "
148  "%u rays total", __first_ray, __last_ray, __front_ray,
149  __front_idx, __num_rays);
150  logger->log_info(name(), "Slit Division: %u", __slit_division);
151  logger->log_info(name(), "Step/Angle: %f", __step_per_angle);
152  logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step);
153  logger->log_info(name(), "Angular Range: %f deg", __angular_range);
154 
155  alloc_distances(__number_of_values);
156 #ifdef HAVE_URG_GBX_9_11
157  __laser->SetPower(true);
158 #else
159  __laser->set_power(true);
160 #endif
161 
162  laser.release();
163 }
164 
165 
166 void
168 {
169  free(_distances);
170  _distances = NULL;
171 
172  logger->log_debug(name(), "Stopping laser");
173 #ifdef HAVE_URG_GBX_9_11
174  __laser->SetPower(false);
175 #else
176  __laser->set_power(false);
177 #endif
178  delete __laser;
179  delete __data;
180 }
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:60
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
Definition: urg_gbx_aqt.cpp:72
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void finalize()
Finalize the thread.
Laser acqusition thread.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
virtual void loop()
Code to execute in the thread.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
unsigned int _distances_size
Assign this the size of the _distances array.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
virtual void init()
Initialize the thread.
Definition: urg_gbx_aqt.cpp:83
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
Interface for configuration handling.
Definition: config.h:67
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Interface for logging.
Definition: logger.h:34