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
41using namespace hokuyo_aist;
43using namespace hokuyoaist;
59 std::string &cfg_prefix)
62 set_name(
"HokuyoURG_GBX(%s)", cfg_name.c_str());
63 pre_init_done_ =
false;
65 cfg_prefix_ = cfg_prefix;
76 pre_init_done_ =
true;
86#ifdef HAVE_URG_GBX_9_11
87 laser_ =
new HokuyoLaser();
88# if __cplusplus >= 201103L
89 std::unique_ptr<HokuyoLaser> laser(laser_);
91 std::auto_ptr<HokuyoLaser> laser(laser_);
94 laser_ =
new Sensor();
95# if __cplusplus >= 201103L
96 std::unique_ptr<Sensor> laser(laser_);
98 std::auto_ptr<Sensor> laser(laser_);
101 std::string port_options =
"type=serial,device=" + cfg_device_ +
",timeout=1";
103#ifdef HAVE_URG_GBX_9_11
104 laser_->Open(port_options);
106 laser_->open(port_options);
108 }
catch (flexiport::PortException &e) {
109 throw Exception(
"Connecting to URG laser failed: %s", e.what());
112#ifdef HAVE_URG_GBX_9_11
113 HokuyoSensorInfo info;
114 laser_->GetSensorInfo(&info);
116 data_ =
new HokuyoData();
117 first_ray_ = info.firstStep;
118 last_ray_ = info.lastStep;
119 front_ray_ = info.frontStep;
123 laser_->get_sensor_info(info);
124 data_ =
new ScanData();
126 first_ray_ = info.first_step;
127 last_ray_ = info.last_step;
128 front_ray_ = info.front_step;
131 slit_division_ = info.steps;
132 num_rays_ = last_ray_ - first_ray_;
133 front_idx_ = front_ray_ - first_ray_;
135 step_per_angle_ = slit_division_ / 360.;
136 angle_per_step_ = 360. / slit_division_;
137 angular_range_ = (last_ray_ - first_ray_) * angle_per_step_;
145 "Rays range: %u..%u, front at %u (idx %u), "
158#ifdef HAVE_URG_GBX_9_11
159 laser_->SetPower(
true);
161 laser_->set_power(
true);
174#ifdef HAVE_URG_GBX_9_11
175 laser_->SetPower(
false);
177 laser_->set_power(
false);
201#ifdef HAVE_URG_GBX_9_11
202 laser_->GetRanges(data_);
203 }
catch (HokuyoError &he) {
205 laser_->get_ranges(*data_);
206 }
catch (BaseError &he) {
212#ifdef HAVE_URG_GBX_9_11
213 const uint32_t *ranges = data_->Ranges();
215 const uint32_t *ranges = data_->ranges();
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_) {
HokuyoUrgGbxAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
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.
Interface for configuration handling.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
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.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
const char * name() const
Get name of thread.
void set_name(const char *format,...)
Set name of thread.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.