00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00071
00072
00073
00074
00075
00076
00077 class LaserDeadSpotCalibrator : public BlackBoardInterfaceListener
00078 {
00079 public:
00080
00081
00082
00083
00084
00085
00086
00087
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
00124 void
00125 wait_finished()
00126 {
00127 __start_measuring = true;
00128 __finish_waitcond.wait();
00129 }
00130
00131
00132
00133
00134 std::vector<std::pair<float, float> >
00135 get_dead_spots()
00136 {
00137 return __dead_spots;
00138 }
00139
00140
00141
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
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
00189 float start_angle = i * angle_factor;
00190
00191
00192
00193 do {
00194
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
00212 __dead_spots.push_back(std::make_pair(start_angle, end_angle));
00213 } else {
00214
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
00232 if (__dead_spots[ind1].first > __dead_spots[ind2].second) {
00233
00234
00235
00236
00237
00238
00239 __dead_spots[ind1].second = 360.;
00240 __dead_spots[ind2].first = 0.;
00241 } else {
00242
00243
00244
00245
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
00258 unsigned int i = 0;
00259 while (i < __dead_spots.size() - 1) {
00260
00261
00262 if (merge_region(i, i+1)) ++i;
00263 }
00264
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
00276 if (__margin != 0.0) {
00277
00278
00279 for (unsigned int i = 0; i != __dead_spots.size(); ++i) {
00280
00281
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
00288
00289
00290 }
00291
00292 merge_spots();
00293 }
00294 }
00295
00296 void
00297 normalize()
00298 {
00299
00300
00301 for (unsigned int i = 0; i != __dead_spots.size(); ++i) {
00302 if (__dead_spots[i].first < 0.) {
00303
00304
00305 __dead_spots[i].first = 360. + __dead_spots[i].first;
00306 }
00307 if (__dead_spots[i].second < 0.) {
00308
00309
00310 __dead_spots[i].second = 360. + __dead_spots[i].first;
00311 }
00312
00313 if (__dead_spots[i].first > __dead_spots[i].second) {
00314
00315
00316
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
00325
00326 }
00327 }
00328
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
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
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
00570
00571 Configuration::ValueIterator *vit = netconf->search("/hardware/laser/dead_spots/");
00572 while (vit->next()) {
00573
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
00613
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 }