Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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
73  fawkes::Logger *logger)
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  std::auto_ptr<HokuyoLaser> laser(__laser);
92 #else
93  __laser = new Sensor();
94  std::auto_ptr<Sensor> laser(__laser);
95 #endif
96  std::string port_options = "type=serial,device=" + __cfg_device + ",timeout=1";
97  try {
98 #ifdef HAVE_URG_GBX_9_11
99  __laser->Open(port_options);
100 #else
101  __laser->open(port_options);
102 #endif
103  } catch (flexiport::PortException &e) {
104  throw Exception("Connecting to URG laser failed: %s", e.what());
105  }
106 
107 #ifdef HAVE_URG_GBX_9_11
108  HokuyoSensorInfo info;
109  __laser->GetSensorInfo(&info);
110 
111  __data = new HokuyoData();
112  __first_ray = info.firstStep;
113  __last_ray = info.lastStep;
114  __front_ray = info.frontStep;
115 
116 #else
117  SensorInfo info;
118  __laser->get_sensor_info(info);
119  __data = new ScanData();
120 
121  __first_ray = info.first_step;
122  __last_ray = info.last_step;
123  __front_ray = info.front_step;
124 #endif
125 
126  __slit_division = info.steps;
127  __num_rays = __last_ray - __first_ray;
128  __front_idx = __front_ray - __first_ray;
129 
130  __step_per_angle = __slit_division / 360.;
131  __angle_per_step = 360. / __slit_division;
132  __angular_range = (__last_ray - __first_ray) * __angle_per_step;
133 
134  logger->log_info(name(), "VEND: %s", info.vendor.c_str());
135  logger->log_info(name(), "PROD: %s", info.product.c_str());
136  logger->log_info(name(), "FIRM: %s", info.firmware.c_str());
137  logger->log_info(name(), "PROT: %s", info.protocol.c_str());
138  logger->log_info(name(), "SERI: %s", info.serial.c_str());
139  logger->log_info(name(), "Rays range: %u..%u, front at %u (idx %u), "
140  "%u rays total", __first_ray, __last_ray, __front_ray,
141  __front_idx, __num_rays);
142  logger->log_info(name(), "Slit Division: %u", __slit_division);
143  logger->log_info(name(), "Step/Angle: %f", __step_per_angle);
144  logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step);
145  logger->log_info(name(), "Angular Range: %f deg", __angular_range);
146 
147  alloc_distances(__number_of_values);
148 #ifdef HAVE_URG_GBX_9_11
149  __laser->SetPower(true);
150 #else
151  __laser->set_power(true);
152 #endif
153 
154  laser.release();
155 }
156 
157 
158 void
160 {
161  free(_distances);
162  _distances = NULL;
163 
164  logger->log_debug(name(), "Stopping laser");
165 #ifdef HAVE_URG_GBX_9_11
166  __laser->SetPower(false);
167 #else
168  __laser->set_power(false);
169 #endif
170  delete __laser;
171  delete __data;
172 }
173 
174 
175 void
177 {
178  // static Time ref(clock);
179  // static Time now(clock);
180  // static unsigned int scans = 0;
181 
182  // now.stamp();
183  // if (now - &ref >= 1) {
184  // logger->log_debug(name(), "Current: %u scans/sec", scans);
185  // scans = 0;
186  // ref = now;
187  // } else {
188  // ++scans;
189  // }
190 
191  try {
192  // GetNewRanges is causes scans/sec to be halfed
193 #ifdef HAVE_URG_GBX_9_11
194  __laser->GetRanges(__data);
195  } catch (HokuyoError &he) {
196 #else
197  __laser->get_ranges(*__data);
198  } catch (BaseError &he) {
199 #endif
200  logger->log_warn(name(), "Failed to read data: %s", he.what());
201  return;
202  }
203 
204 #ifdef HAVE_URG_GBX_9_11
205  const uint32_t *ranges = __data->Ranges();
206 #else
207  const uint32_t *ranges = __data->ranges();
208 #endif
209 
210  _data_mutex->lock();
211 
212  _new_data = true;
213  for (unsigned int a = 0; a < 360; ++a) {
214  unsigned int frontrel_idx = __front_idx + roundf(a * __step_per_angle);
215  unsigned int idx = frontrel_idx % __slit_division;
216  if ( idx <= __num_rays ) {
217  // div by 1000.f: mm -> m
218  _distances[a] = ranges[idx] / 1000.f;
219  }
220  }
221  _data_mutex->unlock();
222 }
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.
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::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:749
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.
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.
Definition: thread.h:95
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
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
Interface for configuration handling.
Definition: config.h:63
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