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