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
41using namespace hokuyo_aist;
42#else
43using namespace hokuyoaist;
44#endif
45using 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
68void
70{
71 if (pre_init_done_)
72 return;
73
74 number_of_values_ = _distances_size = 360;
75
76 pre_init_done_ = true;
77}
78
79void
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());
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
167void
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
183void
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 }
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:68
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.