Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * deadspots.cpp - Laser dead spots calibration tool 00004 * 00005 * Created: Wed Jun 24 12:00:54 2009 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include <core/threading/thread.h> 00024 #include <core/threading/wait_condition.h> 00025 #include <utils/system/argparser.h> 00026 #include <utils/time/time.h> 00027 #include <netcomm/fawkes/client.h> 00028 #include <blackboard/remote.h> 00029 #include <blackboard/interface_listener.h> 00030 #include <config/netconf.h> 00031 00032 #include <interfaces/Laser360Interface.h> 00033 #include <interfaces/Laser720Interface.h> 00034 00035 #include <cstring> 00036 #include <cstdlib> 00037 #include <cstdio> 00038 #include <unistd.h> 00039 #include <cmath> 00040 #include <vector> 00041 #include <algorithm> 00042 #include <utility> 00043 00044 #define MAX_WAIT_TIME 60 00045 #define DEFAULT_WAIT_TIME 10 00046 #define DEFAULT_NUM_MEASUREMENTS 100 00047 #define DEFAULT_COMPARE_DISTANCE 0.9 00048 00049 #define INITIAL_MEASUREMENT 123456.0 00050 00051 using namespace fawkes; 00052 00053 void 00054 print_usage(const char *program_name) 00055 { 00056 printf("Usage: %s [-h] [-r host[:port]] <num_spots>\n" 00057 " -h This help message\n" 00058 " -r host[:port] Remote host (and optionally port) to connect to\n" 00059 " -n <NUM> Number of measurements to use, defaults to %u\n" 00060 " -w <SEC> Wait time in seconds, defaults to %u\n" 00061 " -c <DIST> Compare distance in m, defaults to %f\n" 00062 " -m <MARGIN_DEG> Margin in degree to add around dead spot regions\n" 00063 " -d Dry-run, do not save results to configuration\n" 00064 " -b Show data by opening a blackboard laser interface\n" 00065 "<num_spots> Expected number of dead spots\n", 00066 program_name, DEFAULT_NUM_MEASUREMENTS, DEFAULT_WAIT_TIME, 00067 DEFAULT_COMPARE_DISTANCE); 00068 } 00069 00070 /** Calibrator for dead ranges. 00071 * Depending how the laser is mounted parts of the range it covers might be 00072 * useless data, for example if hidden behind rods. This calibrator detects 00073 * those ranges and writes the information to the config suitable to be 00074 * used by the LaserDeadSpotsDataFilter. 00075 * @author Tim Niemueller 00076 */ 00077 class LaserDeadSpotCalibrator : public BlackBoardInterfaceListener 00078 { 00079 public: 00080 /** Constructor. 00081 * @param num_spots number of expected spots 00082 * @param num_measurements number of measurements to take 00083 * @param compare_distance distance to compare values to 00084 * @param margin extra margin in degree to add around detected regions 00085 * @param blackboard blackboard to register with as listener 00086 * @param laser360 360 beams laser interface 00087 * @param laser720 720 beams laser interface 00088 */ 00089 LaserDeadSpotCalibrator(unsigned int num_spots, unsigned int num_measurements, 00090 float compare_distance, float margin, 00091 BlackBoard *blackboard, 00092 Laser360Interface *laser360, Laser720Interface *laser720) 00093 : BlackBoardInterfaceListener("LaserDeadSpotCalibrator") 00094 { 00095 __laser720 = laser720; 00096 __laser360 = laser360; 00097 __blackboard = blackboard; 00098 __num_spots_expected = num_spots; 00099 __num_measurements = num_measurements; 00100 __cur_measurement = 0; 00101 __num_beams = 0; 00102 __margin = margin; 00103 __compare_distance = compare_distance; 00104 __measurements.clear(); 00105 __num_spots_found = 0; 00106 00107 if (!__laser720 || ! __laser720->has_writer()) { 00108 __lowres_calibrate = true; 00109 __num_beams = __laser360->maxlenof_distances(); 00110 bbil_add_data_interface(__laser360); 00111 } else { 00112 __lowres_calibrate = false; 00113 __num_beams = __laser720->maxlenof_distances(); 00114 bbil_add_data_interface(__laser720); 00115 } 00116 std::vector<float> tmp; 00117 tmp.resize(__num_measurements, INITIAL_MEASUREMENT); 00118 __measurements.resize(__num_beams, tmp); 00119 00120 __blackboard->register_listener(this, BlackBoard::BBIL_FLAG_DATA); 00121 } 00122 00123 /** Wait for the calibration to be finished. */ 00124 void 00125 wait_finished() 00126 { 00127 __start_measuring = true; 00128 __finish_waitcond.wait(); 00129 } 00130 00131 /** Get spots. 00132 * @return vector of detected dead regions 00133 */ 00134 std::vector<std::pair<float, float> > 00135 get_dead_spots() 00136 { 00137 return __dead_spots; 00138 } 00139 00140 /** Get number of spots. 00141 * @return number of spots 00142 */ 00143 unsigned int 00144 num_detected_spots() 00145 { 00146 return __num_spots_found; 00147 } 00148 00149 private: 00150 float 00151 calculate_median(std::vector<float> measurements) 00152 { 00153 std::sort(measurements.begin(), measurements.end()); 00154 return measurements[measurements.size() / 2]; 00155 } 00156 00157 std::vector<float> 00158 calculate_medians() 00159 { 00160 std::vector<float> rv; 00161 rv.resize(__num_beams, INITIAL_MEASUREMENT); 00162 00163 for (unsigned int i = 0; i < __measurements.size(); ++i) { 00164 rv[i] = calculate_median(__measurements[i]); 00165 } 00166 00167 return rv; 00168 } 00169 00170 00171 void 00172 analyze() 00173 { 00174 //printf("ANALYZING\n"); 00175 float angle_factor = 360.0 / __num_beams; 00176 00177 std::vector<float> medians = calculate_medians(); 00178 00179 bool iteration_done = false; 00180 for (unsigned int i = 0; ! iteration_done && i < medians.size(); ++i) { 00181 00182 if (medians[i] == INITIAL_MEASUREMENT) { 00183 printf("WARNING: No valid measurement at angle %f°!\n", i * angle_factor); 00184 continue; 00185 } 00186 00187 if (medians[i] < __compare_distance) { 00188 // start of spot, look for end 00189 float start_angle = i * angle_factor; 00190 00191 //printf("Region starting at %f\n", start_angle); 00192 00193 do { 00194 //printf("Median %u: %f < %f\n", i, medians[i], __compare_distance); 00195 00196 if ((i + 1) >= medians.size()) { 00197 if (iteration_done) { 00198 printf("Could not find end for region starting at %f°, all values " 00199 "too short?\n", start_angle); 00200 break; 00201 } else { 00202 iteration_done = true; 00203 i = 0; 00204 } 00205 } else { 00206 ++i; 00207 } 00208 } while ((medians[i] < __compare_distance) && (medians[i] != INITIAL_MEASUREMENT)); 00209 if (medians[i] >= __compare_distance) { 00210 float end_angle = i * angle_factor; 00211 //printf("Region ends at %f\n", end_angle); 00212 __dead_spots.push_back(std::make_pair(start_angle, end_angle)); 00213 } else { 00214 // did not find end of region 00215 break; 00216 } 00217 } 00218 } 00219 } 00220 00221 void 00222 sort_spots() 00223 { 00224 std::sort(__dead_spots.begin(), __dead_spots.end()); 00225 } 00226 00227 bool 00228 merge_region(unsigned int ind1, unsigned int ind2) 00229 { 00230 if (__dead_spots[ind1].second >= __dead_spots[ind2].first) { 00231 // regions overlap, merge! 00232 if (__dead_spots[ind1].first > __dead_spots[ind2].second) { 00233 // merging would create a region across the discontinuity, do a 00234 // split-merge, i.e. join regions to one, but save as two (cf. normalize()) 00235 //printf("Merging overlapping regions %u [%f, %f] and %u [%f, %f] to [%f, %f]/[%f, %f]\n", 00236 // ind1, __dead_spots[ind1].first, __dead_spots[ind1].second, 00237 // ind2, __dead_spots[ind2].first, __dead_spots[ind2].second, 00238 // __dead_spots[ind1].first, 360., 0., __dead_spots[ind2].second); 00239 __dead_spots[ind1].second = 360.; 00240 __dead_spots[ind2].first = 0.; 00241 } else { 00242 //printf("Merging overlapping regions %u [%f, %f] and %u [%f, %f] to [%f, %f]\n", 00243 // ind1, __dead_spots[ind1].first, __dead_spots[ind1].second, 00244 // ind2, __dead_spots[ind2].first, __dead_spots[ind2].second, 00245 // __dead_spots[ind1].first, __dead_spots[ind2].second); 00246 __dead_spots[ind1].second = __dead_spots[ind2].second; 00247 __dead_spots.erase(__dead_spots.begin() + ind2); 00248 return false; 00249 } 00250 } 00251 return true; 00252 } 00253 00254 void 00255 merge_spots() 00256 { 00257 //printf("MERGING\n"); 00258 unsigned int i = 0; 00259 while (i < __dead_spots.size() - 1) { 00260 //printf("Comparing %u, %u, %f >= %f, %zu\n", i, i+1, 00261 // __dead_spots[i].second, __dead_spots[i+1].first, __dead_spots.size()); 00262 if (merge_region(i, i+1)) ++i; 00263 } 00264 // now check for posssible merge of first and last region (at the discontinuity 00265 unsigned int last = __dead_spots.size() - 1; 00266 if ((__dead_spots[last].second >= __dead_spots[0].first) && (__dead_spots[last].second <= __dead_spots[0].second) && 00267 (__dead_spots[0].first >= __dead_spots[last].first - 360) && (__dead_spots[0].second <= __dead_spots[last].second)) { 00268 merge_region(last, 0); 00269 } 00270 } 00271 00272 void 00273 apply_margin() 00274 { 00275 //printf("MARGIN\n"); 00276 if (__margin != 0.0) { 00277 // post-process, add margins, possibly causing regions to be merged 00278 // add margins 00279 for (unsigned int i = 0; i != __dead_spots.size(); ++i) { 00280 //float before_start = __dead_spots[i].first; 00281 //float before_end = __dead_spots[i].second; 00282 __dead_spots[i].first -= __margin; 00283 __dead_spots[i].second += __margin; 00284 if (__dead_spots[i].second > 360.0) { 00285 __dead_spots[i].second -= 360.0; 00286 } 00287 //printf("Applying margin to spot %i, [%f, %f] -> [%f, %f]\n", 00288 // i, before_start, before_end, 00289 // __dead_spots[i].first, __dead_spots[i].second); 00290 } 00291 // look if regions need to be merged 00292 merge_spots(); 00293 } 00294 } 00295 00296 void 00297 normalize() 00298 { 00299 //printf("NORMALIZING\n"); 00300 // normalize 00301 for (unsigned int i = 0; i != __dead_spots.size(); ++i) { 00302 if (__dead_spots[i].first < 0.) { 00303 //printf("Normalizing %i start angle %f -> %f\n", i, 00304 // __dead_spots[i].first, 360. + __dead_spots[i].first); 00305 __dead_spots[i].first = 360. + __dead_spots[i].first; 00306 } 00307 if (__dead_spots[i].second < 0.) { 00308 //printf("Normalizing %i end angle %f -> %f\n", i, 00309 // __dead_spots[i].second, 360. + __dead_spots[i].second); 00310 __dead_spots[i].second = 360. + __dead_spots[i].first; 00311 } 00312 00313 if (__dead_spots[i].first > __dead_spots[i].second) { 00314 // range over the discontinuity at 0°/360°, split into two regions 00315 //printf("Splitting (size %zu) region %i from [%f, %f] ", __dead_spots.size(), i, 00316 // __dead_spots[i].first, __dead_spots[i].second); 00317 __dead_spots.resize(__dead_spots.size() + 1); 00318 for (int j = __dead_spots.size()-1; j >= (int)i; --j) { 00319 __dead_spots[j+1] = __dead_spots[j]; 00320 } 00321 __dead_spots[i+1].first = 0; 00322 __dead_spots[i].second = 360.0; 00323 00324 //printf("to [%f, %f] and [%f, %f] (size %zu)\n", __dead_spots[i].first, __dead_spots[i].second, 00325 // __dead_spots[i+1].first, __dead_spots[i+1].second, __dead_spots.size()); 00326 } 00327 } 00328 //print_spots(); 00329 sort_spots(); 00330 merge_spots(); 00331 } 00332 00333 00334 void 00335 print_spots() 00336 { 00337 for (unsigned int i = 0; i != __dead_spots.size(); ++i) { 00338 printf("Spot %u start: %3.2f end: %3.2f\n", i, 00339 __dead_spots[i].first, __dead_spots[i].second); 00340 } 00341 } 00342 00343 void 00344 process_measurements() 00345 { 00346 analyze(); 00347 00348 if (__dead_spots.size() > 0) { 00349 apply_margin(); 00350 print_spots(); 00351 00352 __num_spots_found = __dead_spots.size(); 00353 normalize(); 00354 } else { 00355 __num_spots_found = 0; 00356 } 00357 00358 if (__num_spots_found != __num_spots_expected) { 00359 printf("Error, expected %u dead spots, but detected %u.\n", 00360 __num_spots_expected, __num_spots_found); 00361 print_spots(); 00362 } else { 00363 printf("Found expected number of %u dead spots\n", __num_spots_expected); 00364 if (__dead_spots.size() > __num_spots_expected) { 00365 printf("Note that more regions will be printed than spots were expected.\n" 00366 "This is due to splitting that occurs around the discontinuity at 0°/360°\n"); 00367 } 00368 if (__num_spots_expected > __dead_spots.size()) { 00369 printf("Note that less regions will be printed than spots were expected.\n" 00370 "This is due to merging that occurred after applying the margin you\n" 00371 "suggested and normalizing the data.\n"); 00372 } 00373 print_spots(); 00374 } 00375 } 00376 00377 virtual void 00378 bb_interface_data_changed(Interface *interface) throw() 00379 { 00380 if (! __start_measuring) return; 00381 00382 printf("\r%3u samples remaining...", __num_measurements - __cur_measurement); 00383 fflush(stdout); 00384 00385 float *distances = NULL; 00386 unsigned int num_distances = 0; 00387 if (__lowres_calibrate) { 00388 __laser360->read(); 00389 distances = __laser360->distances(); 00390 num_distances = __laser360->maxlenof_distances(); 00391 } else { 00392 __laser720->read(); 00393 distances = __laser720->distances(); 00394 num_distances = __laser720->maxlenof_distances(); 00395 } 00396 00397 for (unsigned int i = 0; i < num_distances; ++i) { 00398 if (distances[i] > 0.0) { 00399 __measurements[i][__cur_measurement] = distances[i]; 00400 } 00401 } 00402 00403 if (++__cur_measurement >= __num_measurements) { 00404 printf("\rMeasuring done, post-processing data now.\n"); 00405 process_measurements(); 00406 __blackboard->unregister_listener(this); 00407 __finish_waitcond.wake_all(); 00408 } 00409 } 00410 00411 private: 00412 BlackBoard *__blackboard; 00413 Laser360Interface *__laser360; 00414 Laser720Interface *__laser720; 00415 WaitCondition __finish_waitcond; 00416 00417 float __margin; 00418 bool __lowres_calibrate; 00419 bool __start_measuring; 00420 unsigned int __num_spots_expected; 00421 unsigned int __num_beams; 00422 unsigned int __num_measurements; 00423 unsigned int __cur_measurement; 00424 unsigned int __num_spots_found; 00425 float __compare_distance; 00426 std::vector<std::vector<float> > __measurements; 00427 std::vector<std::pair<float, float> > __dead_spots; 00428 }; 00429 00430 int 00431 main(int argc, char **argv) 00432 { 00433 ArgumentParser argp(argc, argv, "hr:n:w:c:m:bd"); 00434 00435 if ( argp.has_arg("h") ) { 00436 print_usage(argv[0]); 00437 exit(0); 00438 } 00439 00440 char *host = (char *)"localhost"; 00441 unsigned short int port = FAWKES_TCP_PORT; 00442 long int num_measurements = DEFAULT_NUM_MEASUREMENTS; 00443 long int wait_time = DEFAULT_WAIT_TIME; 00444 float compare_distance = DEFAULT_COMPARE_DISTANCE; 00445 float margin = 0; 00446 00447 if (argp.has_arg("n")) { 00448 num_measurements = argp.parse_int("n"); 00449 if (num_measurements <= 0) { 00450 printf("Invalid number of measurements, must be > 0\n\n"); 00451 print_usage(argp.program_name()); 00452 return -4; 00453 } 00454 } 00455 if (argp.has_arg("w")) { 00456 wait_time = argp.parse_int("w"); 00457 if (wait_time < 0) { 00458 printf("Invalid wait time, must be integer > 0\n\n"); 00459 print_usage(argp.program_name()); 00460 return -4; 00461 } else if (wait_time > MAX_WAIT_TIME) { 00462 printf("Wait time of more than %u seconds are nonsense, aborting.\n\n", MAX_WAIT_TIME); 00463 print_usage(argp.program_name()); 00464 return -4; 00465 } 00466 } 00467 if (argp.has_arg("c")) { 00468 compare_distance = argp.parse_float("c"); 00469 if (compare_distance < 0) { 00470 printf("Invalid compare distance, must be > 0\n\n"); 00471 print_usage(argp.program_name()); 00472 return -4; 00473 } 00474 } 00475 if (argp.has_arg("m")) { 00476 margin = argp.parse_int("m"); 00477 if ((margin <= -360) || (margin >= 360)) { 00478 printf("Invalid margin, must be in the ragen [-359, 359]\n\n"); 00479 print_usage(argp.program_name()); 00480 return -4; 00481 } 00482 } 00483 if (argp.num_items() != 1) { 00484 printf("Number of expected dead spots not supplied\n\n"); 00485 print_usage(argp.program_name()); 00486 return -4; 00487 } 00488 bool free_host = argp.parse_hostport("r", &host, &port); 00489 00490 FawkesNetworkClient *client; 00491 BlackBoard *blackboard; 00492 NetworkConfiguration *netconf; 00493 00494 try { 00495 client = new FawkesNetworkClient(host, port); 00496 client->connect(); 00497 blackboard = new RemoteBlackBoard(client); 00498 netconf = new NetworkConfiguration(client); 00499 } catch (Exception &e) { 00500 printf("Failed to connect to remote host at %s:%u\n\n", host, port); 00501 e.print_trace(); 00502 return -1; 00503 } 00504 00505 if ( free_host ) free(host); 00506 00507 Laser360Interface *laser360 = NULL; 00508 Laser720Interface *laser720 = NULL; 00509 try { 00510 laser360 = blackboard->open_for_reading<Laser360Interface>("Laser"); 00511 laser720 = blackboard->open_for_reading<Laser720Interface>("Laser"); 00512 } catch (Exception &e) { 00513 printf("Failed to open blackboard interfaces"); 00514 e.print_trace(); 00515 //return -2; 00516 } 00517 00518 if (! laser720->has_writer() && ! laser360->has_writer() ) { 00519 printf("No writer for neither high nor low resolution laser.\n" 00520 "Laser plugin not loaded?\n\n"); 00521 blackboard->close(laser360); 00522 blackboard->close(laser720); 00523 //return -3; 00524 } 00525 00526 if (! laser720->has_writer()) { 00527 printf("Warning: high resolution laser not found calibrating with 1° resolution.\n" 00528 " It is recommended to enable the high resolution mode for\n" 00529 " calibration. Acquired 1° data may be unsuitable when used in\n" 00530 " high resolution mode!\n\n"); 00531 blackboard->close(laser720); 00532 laser720 = NULL; 00533 } 00534 00535 Time now, start; 00536 start.stamp(); 00537 now.stamp(); 00538 00539 printf("Position the laser such that it has %f m of free space around it.\n\n" 00540 "Also verify that the laser is running with disable filters\n\n", compare_distance); 00541 fflush(stdout); 00542 float diff = 0; 00543 while ((diff = (now - &start)) < wait_time) { 00544 printf("\rCalibration will start in %2li sec (Ctrl-C to abort)...", wait_time - (unsigned int)floor(diff)); 00545 fflush(stdout); 00546 usleep(200000); 00547 now.stamp(); 00548 } 00549 printf("\rCalibration starting now. \n"); 00550 00551 unsigned int num_spots = argp.parse_item_int(0); 00552 00553 LaserDeadSpotCalibrator *calib; 00554 calib = new LaserDeadSpotCalibrator(num_spots, num_measurements, compare_distance, margin, 00555 blackboard, laser360, laser720); 00556 calib->wait_finished(); 00557 00558 std::vector<std::pair<float, float> > dead_spots = calib->get_dead_spots(); 00559 00560 if ( ! argp.has_arg("d") ) { 00561 if ( num_spots != calib->num_detected_spots() ) { 00562 printf("Number of spots does not match expectation. Not writing to config file."); 00563 } else { 00564 printf("Storing information in remote config\n"); 00565 00566 netconf->set_mirror_mode(true); 00567 00568 for (unsigned int i = 0; i < 2; ++i) { 00569 // do twice, after erasing host specific values there might be default 00570 // values 00571 Configuration::ValueIterator *vit = netconf->search("/hardware/laser/dead_spots/"); 00572 while (vit->next()) { 00573 //printf("Erasing existing value %s\n", vit->path()); 00574 if (vit->is_default()) { 00575 netconf->erase_default(vit->path()); 00576 } else { 00577 netconf->erase(vit->path()); 00578 } 00579 } 00580 delete vit; 00581 } 00582 00583 for (unsigned int i = 0; i < dead_spots.size(); ++i) { 00584 char *prefix; 00585 if (asprintf(&prefix, "/hardware/laser/dead_spots/%u/", i) == -1) { 00586 printf("Failed to store dead spot %u, out of memory\n", i); 00587 continue; 00588 } 00589 std::string start_path = std::string(prefix) + "start"; 00590 std::string end_path = std::string(prefix) + "end"; 00591 free(prefix); 00592 netconf->set_float(start_path.c_str(), dead_spots[i].first); 00593 netconf->set_float(end_path.c_str(), dead_spots[i].second); 00594 } 00595 } 00596 } 00597 00598 delete calib; 00599 delete netconf; 00600 blackboard->close(laser360); 00601 blackboard->close(laser720); 00602 00603 if (argp.has_arg("b")) { 00604 Laser720Interface *lcalib = blackboard->open_for_writing<Laser720Interface>("Laser Calibration"); 00605 for (unsigned int i = 0; i < 720; ++i) { 00606 lcalib->set_distances(i, 1.0); 00607 } 00608 for (unsigned int i = 0; i != dead_spots.size(); ++i) { 00609 const unsigned int start = (unsigned int)dead_spots[i].first * 2; 00610 unsigned int end = (unsigned int)dead_spots[i].second * 2; 00611 if (end == 720) end = 719; 00612 //printf("Marking dead %f/%u to %f/%u\n", 00613 // dead_spots[i].first, start, dead_spots[i].second, end); 00614 for (unsigned int j = start; j <= end; ++j) { 00615 lcalib->set_distances(j, 0.0); 00616 } 00617 } 00618 lcalib->write(); 00619 printf("Storing data in BlackBoard for visualization. Press Ctrl-C to quit.\n"); 00620 while (1) { 00621 usleep(1000000); 00622 } 00623 } 00624 00625 delete blackboard; 00626 delete client; 00627 00628 return 0; 00629 }