00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "urg_gbx_aqt.h"
00024
00025 #include <core/threading/mutex.h>
00026
00027 #include <hokuyo_aist/hokuyo_aist.h>
00028 #include <flexiport/flexiport.h>
00029
00030 #include <memory>
00031 #include <cstdlib>
00032 #include <cmath>
00033 #include <string>
00034 #include <cstdio>
00035
00036 using namespace hokuyo_aist;
00037 using namespace fawkes;
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 HokuyoUrgGbxAcquisitionThread::HokuyoUrgGbxAcquisitionThread(std::string &cfg_name,
00053 std::string &cfg_prefix)
00054 : LaserAcquisitionThread("HokuyoUrgGbxAcquisitionThread")
00055 {
00056 set_name("HokuyoURG_GBX(%s)", cfg_name.c_str());
00057 __pre_init_done = false;
00058 __cfg_name = cfg_name;
00059 __cfg_prefix = cfg_prefix;
00060 }
00061
00062
00063 void
00064 HokuyoUrgGbxAcquisitionThread::pre_init(fawkes::Configuration *config,
00065 fawkes::Logger *logger)
00066 {
00067 if (__pre_init_done) return;
00068
00069 __number_of_values = _distances_size = 360;
00070
00071 __pre_init_done = true;
00072 }
00073
00074 void
00075 HokuyoUrgGbxAcquisitionThread::init()
00076 {
00077 pre_init(config, logger);
00078
00079 __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
00080
00081 __laser = new HokuyoLaser();
00082 std::auto_ptr<HokuyoLaser> laser(__laser);
00083 std::string port_options = "type=serial,device=" + __cfg_device + ",timeout=1";
00084 try {
00085 __laser->Open(port_options);
00086 } catch (flexiport::PortException &e) {
00087 throw Exception("Connecting to URG laser failed: %s", e.what());
00088 }
00089
00090 HokuyoSensorInfo info;
00091 __laser->GetSensorInfo(&info);
00092 __data = new HokuyoData();
00093
00094 __first_ray = info.firstStep;
00095 __last_ray = info.lastStep;
00096 __num_rays = __last_ray - __first_ray;
00097 __front_ray = info.frontStep;
00098 __front_idx = __front_ray - __first_ray;
00099 __slit_division = info.steps;
00100
00101 __step_per_angle = __slit_division / 360.;
00102 __angle_per_step = 360. / __slit_division;
00103 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
00104
00105 logger->log_info(name(), "VEND: %s", info.vendor.c_str());
00106 logger->log_info(name(), "PROD: %s", info.product.c_str());
00107 logger->log_info(name(), "FIRM: %s", info.firmware.c_str());
00108 logger->log_info(name(), "PROT: %s", info.protocol.c_str());
00109 logger->log_info(name(), "SERI: %s", info.serial.c_str());
00110 logger->log_info(name(), "Rays range: %u..%u, front at %u (idx %u), "
00111 "%u rays total", __first_ray, __last_ray, __front_ray,
00112 __front_idx, __num_rays);
00113 logger->log_info(name(), "Slit Division: %u", __slit_division);
00114 logger->log_info(name(), "Step/Angle: %f", __step_per_angle);
00115 logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step);
00116 logger->log_info(name(), "Angular Range: %f deg", __angular_range);
00117
00118 alloc_distances(__number_of_values);
00119 __laser->SetPower(true);
00120
00121 laser.release();
00122 }
00123
00124
00125 void
00126 HokuyoUrgGbxAcquisitionThread::finalize()
00127 {
00128 free(_distances);
00129 _distances = NULL;
00130
00131 logger->log_debug(name(), "Stopping laser");
00132 __laser->SetPower(false);
00133 delete __laser;
00134 delete __data;
00135 }
00136
00137
00138 void
00139 HokuyoUrgGbxAcquisitionThread::loop()
00140 {
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154 try {
00155
00156 __laser->GetRanges(__data);
00157 } catch (HokuyoError &he) {
00158 logger->log_warn(name(), "Failed to read data: %s", he.what());
00159 return;
00160 }
00161
00162 const uint32_t *ranges = __data->Ranges();
00163
00164 _data_mutex->lock();
00165
00166 _new_data = true;
00167 for (unsigned int a = 0; a < 360; ++a) {
00168 unsigned int frontrel_idx = __front_idx + roundf(a * __step_per_angle);
00169 unsigned int idx = frontrel_idx % __slit_division;
00170 if ( idx <= __num_rays ) {
00171
00172 _distances[a] = ranges[idx] / 1000.f;
00173 }
00174 }
00175 _data_mutex->unlock();
00176 }