23 #include "urg_gbx_aqt.h" 25 #include <core/threading/mutex.h> 27 #ifdef HAVE_URG_GBX_9_11 28 # include <hokuyo_aist/hokuyo_aist.h> 30 # include <hokuyoaist/hokuyoaist.h> 32 #include <flexiport/flexiport.h> 40 #ifdef HAVE_URG_GBX_9_11 41 using namespace hokuyo_aist;
61 std::string &cfg_prefix)
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;
75 if (__pre_init_done)
return;
79 __pre_init_done =
true;
89 #ifdef HAVE_URG_GBX_9_11 90 __laser =
new HokuyoLaser();
91 std::auto_ptr<HokuyoLaser> laser(__laser);
93 __laser =
new Sensor();
94 std::auto_ptr<Sensor> laser(__laser);
96 std::string port_options =
"type=serial,device=" + __cfg_device +
",timeout=1";
98 #ifdef HAVE_URG_GBX_9_11 99 __laser->Open(port_options);
101 __laser->open(port_options);
103 }
catch (flexiport::PortException &e) {
104 throw Exception(
"Connecting to URG laser failed: %s", e.what());
107 #ifdef HAVE_URG_GBX_9_11 108 HokuyoSensorInfo info;
109 __laser->GetSensorInfo(&info);
111 __data =
new HokuyoData();
112 __first_ray = info.firstStep;
113 __last_ray = info.lastStep;
114 __front_ray = info.frontStep;
118 __laser->get_sensor_info(info);
119 __data =
new ScanData();
121 __first_ray = info.first_step;
122 __last_ray = info.last_step;
123 __front_ray = info.front_step;
126 __slit_division = info.steps;
127 __num_rays = __last_ray - __first_ray;
128 __front_idx = __front_ray - __first_ray;
130 __step_per_angle = __slit_division / 360.;
131 __angle_per_step = 360. / __slit_division;
132 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
140 "%u rays total", __first_ray, __last_ray, __front_ray,
141 __front_idx, __num_rays);
148 #ifdef HAVE_URG_GBX_9_11 149 __laser->SetPower(
true);
151 __laser->set_power(
true);
165 #ifdef HAVE_URG_GBX_9_11 166 __laser->SetPower(
false);
168 __laser->set_power(
false);
193 #ifdef HAVE_URG_GBX_9_11 194 __laser->GetRanges(__data);
195 }
catch (HokuyoError &he) {
197 __laser->get_ranges(*__data);
198 }
catch (BaseError &he) {
204 #ifdef HAVE_URG_GBX_9_11 205 const uint32_t *ranges = __data->Ranges();
207 const uint32_t *ranges = __data->ranges();
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 ) {
HokuyoUrgGbxAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void finalize()
Finalize the thread.
Fawkes library namespace.
void unlock()
Unlock the mutex.
virtual void loop()
Code to execute in 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::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.
Base class for exceptions in Fawkes.
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.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lock()
Lock this mutex.
virtual void init()
Initialize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Interface for configuration handling.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.