00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "sensproc_thread.h"
00026 #include "hough_transform.h"
00027
00028 #include <interfaces/Laser360Interface.h>
00029 #include <interfaces/ObjectPositionInterface.h>
00030 #include <interfaces/VisualDisplay2DInterface.h>
00031
00032 #include <utils/math/angle.h>
00033 #include <utils/math/coord.h>
00034 #ifdef LASERHT_TIMETRACKER
00035 # include <utils/time/tracker.h>
00036 #endif
00037
00038 #include <cstdlib>
00039
00040 using namespace fawkes;
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 LaserHtSensorProcThread::LaserHtSensorProcThread()
00052 : Thread("LaserHtSensorProcThread", Thread::OPMODE_WAITFORWAKEUP),
00053 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
00054 {
00055 }
00056
00057
00058 void
00059 LaserHtSensorProcThread::init()
00060 {
00061 __laser360_if = NULL;
00062 __visdisp_if = NULL;
00063 __line_if = NULL;
00064
00065 __cfg_num_samples = config->get_uint("/plugins/laserht/line/num_samples");
00066 __cfg_r_scale = config->get_float("/plugins/laserht/line/r_scale");
00067 __cfg_laser_ifid = config->get_string("/plugins/laserht/laser_interface_id");
00068 __cfg_enable_disp = config->get_bool("/plugins/laserht/line/enable_display");
00069 __cfg_vote_threshold = config->get_uint("/plugins/laserht/line/vote_threshold");
00070 __cfg_dist_threshold = config->get_float("/plugins/laserht/line/dist_threshold");
00071 __cfg_fitting_error_threshold = config->get_float("/plugins/laserht/line/fitting_error_threshold");
00072
00073 __laser360_if = NULL;
00074 __line_if = NULL;
00075 try {
00076 __laser360_if = blackboard->open_for_reading<Laser360Interface>(__cfg_laser_ifid.c_str());
00077 if (__cfg_enable_disp) {
00078 __visdisp_if = blackboard->open_for_reading<VisualDisplay2DInterface>("LaserGUI");
00079 }
00080 __line_if = blackboard->open_for_writing<ObjectPositionInterface>("LaserLine");
00081 __line_if->set_object_type(ObjectPositionInterface::TYPE_LINE);
00082 } catch (Exception &e) {
00083 blackboard->close(__laser360_if);
00084 blackboard->close(__line_if);
00085 throw;
00086 }
00087
00088 __ht = new HoughTransform(2);
00089
00090 __num_vals = __cfg_num_samples;
00091 __angle_step = 180.f / __num_vals;
00092 __r_scale = __cfg_r_scale;
00093 __values = new int*[__num_vals];
00094 for (unsigned int i = 0; i < __num_vals; ++i) {
00095 __values[i] = new int[2];
00096 }
00097
00098 #ifdef LASERHT_TIMETRACKER
00099 __tt = new TimeTracker();
00100 __tt_loop = 0;
00101 __ttc_reset = __tt->add_class("Reset");
00102 __ttc_process = __tt->add_class("Processing");
00103 __ttc_fitting = __tt->add_class("Fitting");
00104 __ttc_total = __tt->add_class("Total");
00105 #endif
00106 }
00107
00108
00109 void
00110 LaserHtSensorProcThread::finalize()
00111 {
00112 __line_if->set_valid(false);
00113 __line_if->write();
00114
00115 blackboard->close(__laser360_if);
00116 blackboard->close(__visdisp_if);
00117 blackboard->close(__line_if);
00118
00119 delete __ht;
00120 for (unsigned int i = 0; i < __num_vals; ++i) {
00121 delete[] __values[i];
00122 }
00123 delete[] __values;
00124 }
00125
00126
00127 void
00128 LaserHtSensorProcThread::line_points_from_params(float r, float phi,
00129 float &x1, float &y1,
00130 float &x2, float &y2)
00131 {
00132 float phi_rad = deg2rad(phi);
00133 float phi_mod = phi - (floorf(phi / 90.) * 90);
00134 float r_scaled = r * __r_scale;
00135 float tx, ty;
00136 polar2cart2d(phi_rad, r_scaled, &tx, &ty);
00137 x1 = tx;
00138 y1 = ty;
00139
00140 float alpha, y_factor = 1;
00141 if ( ((phi >= 0) && (phi < 90)) ||
00142 (phi >= 270) ) {
00143 y_factor = -1;
00144 alpha = deg2rad(90 - phi_mod);
00145 } else {
00146 alpha = deg2rad(phi_mod);
00147 }
00148 float dx = 1 * cos(alpha);
00149 float dy = 1 * y_factor * sin(alpha);
00150 x2 = x1 + dx;
00151 y2 = y1 + dy;
00152 }
00153
00154
00155 void
00156 LaserHtSensorProcThread::loop()
00157 {
00158 __laser360_if->read();
00159 float *distances = __laser360_if->distances();
00160 const size_t num_dist = __laser360_if->maxlenof_distances();
00161
00162 #ifdef LASERHT_TIMETRACKER
00163 __tt->ping_start(__ttc_total);
00164 __tt->ping_start(__ttc_reset);
00165 #endif
00166 __ht->reset();
00167 #ifdef LASERHT_TIMETRACKER
00168 __tt->ping_end(__ttc_reset);
00169 __tt->ping_start(__ttc_process);
00170 #endif
00171
00172 for (size_t i = 0; i < num_dist; ++i) {
00173
00174 if (distances[i] > 0) {
00175 for (unsigned int j = 0; j < __num_vals; ++j) {
00176 float phi = deg2rad(i);
00177 float theta = deg2rad(j * __angle_step);
00178 float x, y;
00179 polar2cart2d(phi, distances[i], &x, &y);
00180 float r = x * cos(theta) + y * sin(theta);
00181 r /= __r_scale;
00182 __values[j][0] = (int)roundf(r);
00183 __values[j][1] = (int)roundf(j * __angle_step);
00184 }
00185 __ht->process(__values, __num_vals);
00186 }
00187 }
00188 #ifdef LASERHT_TIMETRACKER
00189 __tt->ping_end(__ttc_process);
00190 #endif
00191
00192 int max_values[2];
00193 unsigned int max_count = __ht->max(max_values);
00194
00195 if (max_count >= __cfg_vote_threshold) {
00196 float x1, y1, x2, y2;
00197 line_points_from_params(max_values[0], max_values[1], x1, y1, x2, y2);
00198
00199 try {
00200 if (__cfg_enable_disp && __visdisp_if->has_writer()) {
00201 __visdisp_if->msgq_enqueue(new VisualDisplay2DInterface::DeleteAllMessage());
00202 float x[2] = {x1, x2};
00203 float y[2] = {y1, y2};
00204 unsigned char color[4] = {0, 255, 0, 255};
00205 VisualDisplay2DInterface::AddCartLineMessage *lm;
00206 lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y,
00207 VisualDisplay2DInterface::LS_SOLID, color);
00208 __visdisp_if->msgq_enqueue(lm);
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226 }
00227 } catch (Exception &e) {}
00228
00229
00230 float theta = deg2rad(max_values[1]);
00231 float alpha = 0.5 * M_PI - theta;
00232 float r_scaled = max_values[0] * __r_scale;
00233 float cos_alpha = cos(alpha);
00234 float sin_alpha = sin(alpha);
00235 float threshold = __cfg_dist_threshold;
00236 float r_min = r_scaled - threshold;
00237 float r_max = r_scaled + threshold;
00238
00239 bool first_x_minmax = true;
00240 float x_min = 0, x_max = 0;
00241
00242 std::vector<laser_reading_t> readings;
00243
00244 for (size_t i = 0; i < num_dist; ++i) {
00245
00246 if (distances[i] > 0) {
00247 float x, y;
00248 float phi = deg2rad(i);
00249 polar2cart2d(phi, distances[i], &x, &y);
00250 float r = x * cos(theta) + y * sin(theta);
00251
00252 if ( (r >= r_min) && (r <= r_max) ) {
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273 float x_rot = x * cos_alpha - y * sin_alpha;
00274 float y_rot = x * sin_alpha + y * cos_alpha;
00275
00276 laser_reading_t l = {phi, distances[i], x_rot, y_rot};
00277 readings.push_back(l);
00278 if (first_x_minmax) {
00279 first_x_minmax = false;
00280 x_min = x_rot;
00281 x_max = x_rot;
00282 } else {
00283 if (x_rot < x_min) x_min = x_rot;
00284 if (x_rot > x_max) x_max = x_rot;
00285 }
00286 }
00287 }
00288 }
00289
00290 #ifdef LASERHT_TIMETRACKER
00291 __tt->ping_start(__ttc_fitting);
00292 #endif
00293
00294 float a = 0, b = 0, e = 0;
00295 fit_line(readings, 0, a, b, e);
00296 #ifdef LASERHT_TIMETRACKER
00297 __tt->ping_end(__ttc_fitting);
00298 #endif
00299
00300 if ( e <= __cfg_fitting_error_threshold ) {
00301
00302 float y_min = a * x_min + b;
00303 float y_max = a * x_max + b;
00304
00305
00306
00307
00308 float x_min_rot = x_min * cos_alpha + y_min * sin_alpha;
00309 float y_min_rot = y_min * cos_alpha - x_min * sin_alpha;
00310 float x_max_rot = x_max * cos_alpha + y_max * sin_alpha;
00311 float y_max_rot = y_max * cos_alpha - x_max * sin_alpha;
00312
00313
00314 float alpha_fit = atan2f(y_max_rot - y_min_rot,
00315 x_max_rot - x_min_rot);
00316 if ( (theta <= 0.5 * M_PI) ||
00317 ((theta >= M_PI) && (theta <= 1.5 * M_PI)) ) {
00318 alpha_fit = 0.5 * M_PI + alpha_fit;
00319 }
00320 float theta_fit = floorf(theta / (0.5*M_PI)) * 0.5*M_PI + alpha_fit;
00321 float r_fit = x_min_rot * cos(theta_fit) + y_min_rot * sin(theta_fit);
00322
00323 if (__cfg_enable_disp && __visdisp_if->has_writer()) {
00324 float x1, y1, x2, y2;
00325 line_points_from_params(r_fit / __r_scale, rad2deg(theta_fit),
00326 x1, y1, x2, y2);
00327
00328 try {
00329 __visdisp_if->msgq_enqueue(new VisualDisplay2DInterface::DeleteAllMessage());
00330 float x[2] = {x1, x2};
00331 float y[2] = {y1, y2};
00332 unsigned char color[4] = {0, 0, 255, 255};
00333 VisualDisplay2DInterface::AddCartLineMessage *lm;
00334 lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y,
00335 VisualDisplay2DInterface::LS_SOLID, color);
00336 __visdisp_if->msgq_enqueue(lm);
00337 } catch (Exception &e) {}
00338 }
00339
00340
00341
00342 __line_if->set_world_x(x_min_rot);
00343 __line_if->set_world_y(y_min_rot);
00344
00345 __line_if->set_relative_x(x_max_rot);
00346 __line_if->set_relative_y(y_max_rot);
00347
00348 __line_if->set_bearing(theta_fit);
00349 __line_if->set_distance(r_fit);
00350
00351 __line_if->set_roll(e);
00352 __line_if->set_visible(true);
00353 } else {
00354 logger->log_debug(name(), "Fitting error above threshold: %f > %f",
00355 e, __cfg_fitting_error_threshold);
00356 __line_if->set_roll(e);
00357 __line_if->set_visible(false);
00358 }
00359 } else {
00360 logger->log_debug(name(), "Votes below threshold: %u < %u",
00361 max_count, __cfg_vote_threshold);
00362 __line_if->set_visible(false);
00363 }
00364
00365 __line_if->set_valid(true);
00366 __line_if->write();
00367 #ifdef LASERHT_TIMETRACKER
00368 __tt->ping_end(__ttc_total);
00369 if (++__tt_loop >= 100) {
00370 __tt->print_to_stdout();
00371 __tt_loop = 0;
00372 }
00373 #endif
00374 }
00375
00376 #define sqr(x) ((x) * (x))
00377
00378 void
00379 LaserHtSensorProcThread::fit_line(const std::vector<laser_reading_t> &points,
00380 const unsigned int first_index,
00381 float &a, float &b, float &least_square_error)
00382 {
00383 const size_t n = points.size();
00384 float sum_x = 0.0, sum_xy = 0.0, sum_y = 0.0, sum_xx = 0.0;
00385
00386 float x, y;
00387 register float e = 0.0;
00388 for (size_t i = first_index; i < n; ++i ) {
00389 x = points[i].x;
00390 y = points[i].y;
00391
00392 sum_y += y;
00393 sum_x += x;
00394 sum_xy += x*y;
00395 sum_xx += x*x;
00396
00397 }
00398
00399 b = ( sum_y * sum_xx - sum_x * sum_xy ) / ( n * sum_xx - sum_x * sum_x );
00400 a = ( n * sum_xy - sum_x * sum_y ) / ( n * sum_xx - sum_x * sum_x );
00401
00402
00403 for (size_t i = first_index; i < n; ++i ) {
00404
00405 e += sqr( points[i].y - (points[i].x*a + b) );
00406 }
00407
00408 least_square_error = e;
00409 }