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_aqt.h"
00024
00025 #include <core/threading/mutex.h>
00026 #include <utils/time/wait.h>
00027
00028 #include <urg/UrgCtrl.h>
00029 #include <urg/RangeSensorParameter.h>
00030
00031 #include <memory>
00032 #include <cstdlib>
00033 #include <cmath>
00034 #include <string>
00035 #include <cstdio>
00036
00037 using namespace qrk;
00038 using namespace fawkes;
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 HokuyoUrgAcquisitionThread::HokuyoUrgAcquisitionThread(std::string &cfg_name,
00054 std::string &cfg_prefix)
00055 : LaserAcquisitionThread("HokuyoUrgAcquisitionThread")
00056 {
00057 set_name("HokuyoURG(%s)", cfg_name.c_str());
00058 __pre_init_done = false;
00059 __cfg_name = cfg_name;
00060 __cfg_prefix = cfg_prefix;
00061 }
00062
00063
00064 void
00065 HokuyoUrgAcquisitionThread::pre_init(fawkes::Configuration *config,
00066 fawkes::Logger *logger)
00067 {
00068 if (__pre_init_done) return;
00069
00070 __number_of_values = _distances_size = 360;
00071
00072 __pre_init_done = true;
00073 }
00074
00075 void
00076 HokuyoUrgAcquisitionThread::init()
00077 {
00078 pre_init(config, logger);
00079
00080 __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
00081
00082 __ctrl = new UrgCtrl();
00083 std::auto_ptr<UrgCtrl> ctrl(__ctrl);
00084 if ( ! __ctrl->connect(__cfg_device.c_str()) ) {
00085 throw Exception("Connecting to URG laser failed: %s", __ctrl->what());
00086 }
00087
00088 __ctrl->setCaptureMode(AutoCapture);
00089
00090 std::vector<std::string> version_info;
00091 if (__ctrl->versionLines(version_info)) {
00092 for (unsigned int i = 0; i < version_info.size(); ++i) {
00093 std::string::size_type colon_idx = version_info[i].find(":");
00094 std::string::size_type semi_colon_idx = version_info[i].find(";");
00095 if ((colon_idx == std::string::npos) ||
00096 (semi_colon_idx == std::string::npos)) {
00097 logger->log_warn(name(), "Could not understand version info string '%s'",
00098 version_info[i].c_str());
00099 } else {
00100 std::string::size_type val_len = semi_colon_idx - colon_idx - 1;
00101 std::string key = version_info[i].substr(0, colon_idx);
00102 std::string value = version_info[i].substr(colon_idx+1, val_len);
00103 __device_info[key] = value;
00104 logger->log_info(name(), "%s: %s", key.c_str(), value.c_str());
00105 }
00106 }
00107 } else {
00108 throw Exception("Failed retrieving version info from device: %s", __ctrl->what());
00109 }
00110
00111 if (__device_info.find("PROD") == __device_info.end()) {
00112 throw Exception("Failed to read product info for URG laser");
00113 }
00114
00115 int scan_msec = __ctrl->scanMsec();
00116
00117 try {
00118 __first_ray = config->get_uint((__cfg_prefix + "first_ray").c_str());
00119 __last_ray = config->get_uint((__cfg_prefix + "last_ray").c_str());
00120 __front_ray = config->get_uint((__cfg_prefix + "front_ray").c_str());
00121 __slit_division = config->get_uint((__cfg_prefix + "slit_division").c_str());
00122 } catch (Exception &e) {
00123 logger->log_info(name(), "No or incomplete config data, reading from device");
00124
00125 RangeSensorParameter p = __ctrl->parameter();
00126 __first_ray = p.area_min;
00127 __last_ray = p.area_max;
00128 __front_ray = p.area_front;
00129 __slit_division = p.area_total;
00130 }
00131
00132 __step_per_angle = __slit_division / 360.;
00133 __angle_per_step = 360. / __slit_division;
00134 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
00135
00136 logger->log_info(name(), "Time per scan: %i msec", scan_msec);
00137 logger->log_info(name(), "Rays range: %u..%u, front at %u",
00138 __first_ray, __last_ray, __front_ray);
00139 logger->log_info(name(), "Slit Division: %u", __slit_division);
00140 logger->log_info(name(), "Step/Angle: %f", __step_per_angle);
00141 logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step);
00142 logger->log_info(name(), "Angular Range: %f deg", __angular_range);
00143
00144
00145
00146 __timer = new TimeWait(clock, scan_msec * 990);
00147
00148 alloc_distances(__number_of_values);
00149
00150 ctrl.release();
00151 }
00152
00153
00154 void
00155 HokuyoUrgAcquisitionThread::finalize()
00156 {
00157 free(_distances);
00158 _distances = NULL;
00159 delete __timer;
00160
00161 __ctrl->stop();
00162 delete __ctrl;
00163
00164 logger->log_debug(name(), "Stopping laser");
00165 }
00166
00167
00168 void
00169 HokuyoUrgAcquisitionThread::loop()
00170 {
00171 __timer->mark_start();
00172
00173 std::vector<long> values;
00174 int num_values = __ctrl->capture(values);
00175 if (num_values > 0) {
00176
00177 _data_mutex->lock();
00178
00179 _new_data = true;
00180 for (unsigned int a = 0; a < 360; ++a) {
00181 unsigned int front_idx = __front_ray + roundf(a * __step_per_angle);
00182 unsigned int idx = front_idx % __slit_division;
00183 if ( (idx >= __first_ray) && (idx <= __last_ray) ) {
00184
00185 _distances[a] = values[idx] / 1000.f;
00186 }
00187 }
00188 _data_mutex->unlock();
00189
00190
00191 }
00192
00193 __timer->wait();
00194 }