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 #if __cplusplus >= 201103L 92 std::unique_ptr<HokuyoLaser> laser(__laser);
94 std::auto_ptr<HokuyoLaser> laser(__laser);
97 __laser =
new Sensor();
98 #if __cplusplus >= 201103L 99 std::unique_ptr<Sensor> laser(__laser);
101 std::auto_ptr<Sensor> laser(__laser);
104 std::string port_options =
"type=serial,device=" + __cfg_device +
",timeout=1";
106 #ifdef HAVE_URG_GBX_9_11 107 __laser->Open(port_options);
109 __laser->open(port_options);
111 }
catch (flexiport::PortException &e) {
112 throw Exception(
"Connecting to URG laser failed: %s", e.what());
115 #ifdef HAVE_URG_GBX_9_11 116 HokuyoSensorInfo info;
117 __laser->GetSensorInfo(&info);
119 __data =
new HokuyoData();
120 __first_ray = info.firstStep;
121 __last_ray = info.lastStep;
122 __front_ray = info.frontStep;
126 __laser->get_sensor_info(info);
127 __data =
new ScanData();
129 __first_ray = info.first_step;
130 __last_ray = info.last_step;
131 __front_ray = info.front_step;
134 __slit_division = info.steps;
135 __num_rays = __last_ray - __first_ray;
136 __front_idx = __front_ray - __first_ray;
138 __step_per_angle = __slit_division / 360.;
139 __angle_per_step = 360. / __slit_division;
140 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
148 "%u rays total", __first_ray, __last_ray, __front_ray,
149 __front_idx, __num_rays);
156 #ifdef HAVE_URG_GBX_9_11 157 __laser->SetPower(
true);
159 __laser->set_power(
true);
173 #ifdef HAVE_URG_GBX_9_11 174 __laser->SetPower(
false);
176 __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 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::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.
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.
const char * name() const
Get name of thread.
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.
virtual void init()
Initialize the thread.
Time & stamp()
Set this time to the current time.
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.