deadspots.cpp

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 }