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;
43 using namespace hokuyoaist;
45 using namespace fawkes;
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 ) {