23 #include <core/threading/thread.h> 24 #include <core/threading/wait_condition.h> 25 #include <utils/system/argparser.h> 26 #include <utils/time/time.h> 27 #include <netcomm/fawkes/client.h> 28 #include <blackboard/remote.h> 29 #include <blackboard/interface_listener.h> 30 #include <config/netconf.h> 32 #include <interfaces/Laser360Interface.h> 33 #include <interfaces/Laser720Interface.h> 44 #define MAX_WAIT_TIME 60 45 #define DEFAULT_WAIT_TIME 10 46 #define DEFAULT_NUM_MEASUREMENTS 100 47 #define DEFAULT_COMPARE_DISTANCE 0.9 49 #define INITIAL_MEASUREMENT 123456.0 54 print_usage(
const char *program_name)
56 printf(
"Usage: %s [-h] [-r host[:port]] <num_spots> <config_prefix>\n" 57 " -h This help message\n" 58 " -r host[:port] Remote host (and optionally port) to connect to\n" 59 " -n <NUM> Number of measurements to use, defaults to %u\n" 60 " -w <SEC> Wait time in seconds, defaults to %u\n" 61 " -c <DIST> Compare distance in m, defaults to %f\n" 62 " -m <MARGIN_DEG> Margin in degree to add around dead spot regions\n" 63 " -d Dry-run, do not save results to configuration\n" 64 " -b Show data by opening a blackboard laser interface\n" 65 " -i <ID> Open laser interface named <ID>\n" 66 "<num_spots> Expected number of dead spots\n",
67 program_name, DEFAULT_NUM_MEASUREMENTS, DEFAULT_WAIT_TIME,
68 DEFAULT_COMPARE_DISTANCE);
91 float compare_distance,
float margin,
96 __laser720 = laser720;
97 __laser360 = laser360;
98 __blackboard = blackboard;
99 __num_spots_expected = num_spots;
100 __num_measurements = num_measurements;
101 __cur_measurement = 0;
104 __compare_distance = compare_distance;
105 __measurements.clear();
106 __num_spots_found = 0;
108 if (!__laser720 || ! __laser720->has_writer()) {
109 __lowres_calibrate =
true;
110 __num_beams = __laser360->maxlenof_distances();
111 bbil_add_data_interface(__laser360);
113 __lowres_calibrate =
false;
114 __num_beams = __laser720->maxlenof_distances();
115 bbil_add_data_interface(__laser720);
117 std::vector<float> tmp;
118 tmp.resize(__num_measurements, INITIAL_MEASUREMENT);
119 __measurements.resize(__num_beams, tmp);
121 __blackboard->register_listener(
this);
128 __start_measuring =
true;
129 __finish_waitcond.wait();
135 std::vector<std::pair<float, float> >
147 return __num_spots_found;
152 calculate_median(std::vector<float> measurements)
154 std::sort(measurements.begin(), measurements.end());
155 return measurements[measurements.size() / 2];
161 std::vector<float> rv;
162 rv.resize(__num_beams, INITIAL_MEASUREMENT);
164 for (
unsigned int i = 0; i < __measurements.size(); ++i) {
165 rv[i] = calculate_median(__measurements[i]);
176 float angle_factor = 360.0 / __num_beams;
178 std::vector<float> medians = calculate_medians();
180 bool iteration_done =
false;
181 for (
unsigned int i = 0; ! iteration_done && i < medians.size(); ++i) {
183 if (medians[i] == INITIAL_MEASUREMENT) {
184 printf(
"WARNING: No valid measurement at angle %f°!\n", i * angle_factor);
188 if (medians[i] < __compare_distance) {
190 float start_angle = i * angle_factor;
197 if ((i + 1) >= medians.size()) {
198 if (iteration_done) {
199 printf(
"Could not find end for region starting at %f°, all values " 200 "too short?\n", start_angle);
203 iteration_done =
true;
209 }
while ((medians[i] < __compare_distance) && (medians[i] != INITIAL_MEASUREMENT));
210 if (medians[i] >= __compare_distance) {
211 float end_angle = i * angle_factor;
213 __dead_spots.push_back(std::make_pair(start_angle, end_angle));
225 std::sort(__dead_spots.begin(), __dead_spots.end());
229 merge_region(
unsigned int ind1,
unsigned int ind2)
231 if (__dead_spots[ind1].second >= __dead_spots[ind2].first) {
233 if (__dead_spots[ind1].first > __dead_spots[ind2].second) {
240 __dead_spots[ind1].second = 360.;
241 __dead_spots[ind2].first = 0.;
247 __dead_spots[ind1].second = __dead_spots[ind2].second;
248 __dead_spots.erase(__dead_spots.begin() + ind2);
260 while (i < __dead_spots.size() - 1) {
263 if (merge_region(i, i+1)) ++i;
266 unsigned int last = __dead_spots.size() - 1;
267 if ((__dead_spots[last].second >= __dead_spots[0].first) && (__dead_spots[last].second <= __dead_spots[0].second) &&
268 (__dead_spots[0].first >= __dead_spots[last].first - 360) && (__dead_spots[0].second <= __dead_spots[last].second)) {
269 merge_region(last, 0);
277 if (__margin != 0.0) {
280 for (
unsigned int i = 0; i != __dead_spots.size(); ++i) {
283 __dead_spots[i].first -= __margin;
284 __dead_spots[i].second += __margin;
285 if (__dead_spots[i].second > 360.0) {
286 __dead_spots[i].second -= 360.0;
302 for (
unsigned int i = 0; i != __dead_spots.size(); ++i) {
303 if (__dead_spots[i].first < 0.) {
306 __dead_spots[i].first = 360. + __dead_spots[i].first;
308 if (__dead_spots[i].second < 0.) {
311 __dead_spots[i].second = 360. + __dead_spots[i].first;
314 if (__dead_spots[i].first > __dead_spots[i].second) {
318 __dead_spots.resize(__dead_spots.size() + 1);
319 for (
int j = __dead_spots.size()-1; j >= (int)i; --j) {
320 __dead_spots[j+1] = __dead_spots[j];
322 __dead_spots[i+1].first = 0;
323 __dead_spots[i].second = 360.0;
338 for (
unsigned int i = 0; i != __dead_spots.size(); ++i) {
339 printf(
"Spot %u start: %3.2f end: %3.2f\n", i,
340 __dead_spots[i].first, __dead_spots[i].second);
345 process_measurements()
349 if (__dead_spots.size() > 0) {
353 __num_spots_found = __dead_spots.size();
356 __num_spots_found = 0;
359 if (__num_spots_found != __num_spots_expected) {
360 printf(
"Error, expected %u dead spots, but detected %u.\n",
361 __num_spots_expected, __num_spots_found);
364 printf(
"Found expected number of %u dead spots\n", __num_spots_expected);
365 if (__dead_spots.size() > __num_spots_expected) {
366 printf(
"Note that more regions will be printed than spots were expected.\n" 367 "This is due to splitting that occurs around the discontinuity at 0°/360°\n");
369 if (__num_spots_expected > __dead_spots.size()) {
370 printf(
"Note that less regions will be printed than spots were expected.\n" 371 "This is due to merging that occurred after applying the margin you\n" 372 "suggested and normalizing the data.\n");
379 bb_interface_data_changed(
Interface *interface)
throw()
381 if (! __start_measuring)
return;
383 printf(
"\r%3u samples remaining...", __num_measurements - __cur_measurement);
386 float *distances = NULL;
387 unsigned int num_distances = 0;
388 if (__lowres_calibrate) {
390 distances = __laser360->distances();
391 num_distances = __laser360->maxlenof_distances();
394 distances = __laser720->distances();
395 num_distances = __laser720->maxlenof_distances();
398 for (
unsigned int i = 0; i < num_distances; ++i) {
399 if (finite(distances[i]) && distances[i] > 1e-6) {
400 __measurements[i][__cur_measurement] = distances[i];
404 if (++__cur_measurement >= __num_measurements) {
405 printf(
"\rMeasuring done, post-processing data now.\n");
406 process_measurements();
407 __blackboard->unregister_listener(
this);
408 __finish_waitcond.wake_all();
419 bool __lowres_calibrate;
420 bool __start_measuring;
421 unsigned int __num_spots_expected;
422 unsigned int __num_beams;
423 unsigned int __num_measurements;
424 unsigned int __cur_measurement;
425 unsigned int __num_spots_found;
426 float __compare_distance;
427 std::vector<std::vector<float> > __measurements;
428 std::vector<std::pair<float, float> > __dead_spots;
432 main(
int argc,
char **argv)
437 print_usage(argv[0]);
441 char *host = (
char *)
"localhost";
442 unsigned short int port = FAWKES_TCP_PORT;
443 long int num_measurements = DEFAULT_NUM_MEASUREMENTS;
444 long int wait_time = DEFAULT_WAIT_TIME;
445 float compare_distance = DEFAULT_COMPARE_DISTANCE;
447 std::string interface_id =
"Laser";
448 std::string cfg_prefix =
"";
452 if (num_measurements <= 0) {
453 printf(
"Invalid number of measurements, must be > 0\n\n");
461 printf(
"Invalid wait time, must be integer > 0\n\n");
464 }
else if (wait_time > MAX_WAIT_TIME) {
465 printf(
"Wait time of more than %u seconds are nonsense, aborting.\n\n", MAX_WAIT_TIME);
472 if (compare_distance < 0) {
473 printf(
"Invalid compare distance, must be > 0\n\n");
480 if ((margin <= -360) || (margin >= 360)) {
481 printf(
"Invalid margin, must be in the ragen [-359, 359]\n\n");
487 printf(
"Number of expected dead spots not supplied\n\n");
491 printf(
"Config prefix not given and not dry-run\n\n");
495 printf(
"Too many arguments\n\n");
499 cfg_prefix = argp.
items()[1];
500 if (cfg_prefix[cfg_prefix.length() - 1] !=
'/') {
506 interface_id = argp.
arg(
"i");
520 printf(
"Failed to connect to remote host at %s:%u\n\n", host, port);
525 if ( free_host ) free(host);
533 printf(
"Failed to open blackboard interfaces");
539 printf(
"No writer for neither high nor low resolution laser.\n" 540 "Laser plugin not loaded?\n\n");
541 blackboard->
close(laser360);
542 blackboard->
close(laser720);
547 printf(
"Warning: high resolution laser not found calibrating with 1° resolution.\n" 548 " It is recommended to enable the high resolution mode for\n" 549 " calibration. Acquired 1° data may be unsuitable when used in\n" 550 " high resolution mode!\n\n");
551 blackboard->
close(laser720);
559 printf(
"Position the laser such that it has %f m of free space around it.\n\n" 560 "Also verify that the laser is running with disable filters\n\n", compare_distance);
563 while ((diff = (now - &start)) < wait_time) {
564 printf(
"\rCalibration will start in %2li sec (Ctrl-C to abort)...", wait_time - (
unsigned int)floor(diff));
569 printf(
"\rCalibration starting now. \n");
575 blackboard, laser360, laser720);
578 std::vector<std::pair<float, float> > dead_spots = calib->
get_dead_spots();
582 printf(
"Number of spots does not match expectation. Not writing to config file.");
584 printf(
"Storing information in remote config\n");
588 for (
unsigned int i = 0; i < 2; ++i) {
592 while (vit->
next()) {
603 for (
unsigned int i = 0; i < dead_spots.size(); ++i) {
605 if (asprintf(&prefix,
"%s%u/", cfg_prefix.c_str(), i) == -1) {
606 printf(
"Failed to store dead spot %u, out of memory\n", i);
609 std::string start_path = std::string(prefix) +
"start";
610 std::string end_path = std::string(prefix) +
"end";
612 netconf->
set_float(start_path.c_str(), dead_spots[i].first);
613 netconf->
set_float(end_path.c_str(), dead_spots[i].second);
620 blackboard->
close(laser360);
621 blackboard->
close(laser720);
625 for (
unsigned int i = 0; i < 720; ++i) {
628 for (
unsigned int i = 0; i != dead_spots.size(); ++i) {
629 const unsigned int start = (
unsigned int)dead_spots[i].first * 2;
630 unsigned int end = (
unsigned int)dead_spots[i].second * 2;
631 if (end == 720) end = 719;
634 for (
unsigned int j = start; j <= end; ++j) {
639 printf(
"Storing data in BlackBoard for visualization. Press Ctrl-C to quit.\n");
Laser360Interface Fawkes BlackBoard Interface.
const char * program_name() const
Get name of program.
Wait until a given condition holds.
const char * arg(const char *argn)
Get argument value.
bool parse_hostport(const char *argn, char **host, unsigned short int *port)
Parse host:port string.
double parse_float(const char *argn)
Parse argument as double.
Simple Fawkes network client.
const std::vector< const char *> & items() const
Get non-option items.
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
Fawkes library namespace.
void connect()
Connect to remote.
Parse command line arguments.
A class for handling time.
virtual bool next()=0
Check if there is another element and advance to this if possible.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
unsigned int num_detected_spots()
Get number of spots.
long int parse_int(const char *argn)
Parse argument as integer.
void wait_finished()
Wait for the calibration to be finished.
Calibrator for dead ranges.
std::vector< const char *>::size_type num_items() const
Get number of non-option items.
Base class for exceptions in Fawkes.
LaserDeadSpotCalibrator(unsigned int num_spots, unsigned int num_measurements, float compare_distance, float margin, BlackBoard *blackboard, Laser360Interface *laser360, Laser720Interface *laser720)
Constructor.
virtual void erase(const char *path)
Erase the given value from the configuration.
bool has_writer() const
Check if there is a writer for the interface.
std::vector< std::pair< float, float > > get_dead_spots()
Get spots.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void set_mirror_mode(bool mirror)
Enable or disable mirror mode.
virtual const char * path() const =0
Path of value.
void print_trace()
Prints trace to stderr.
Iterator interface to iterate over config values.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual bool is_default() const =0
Check if current value was read from the default config.
Time & stamp()
Set this time to the current time.
bool has_arg(const char *argn)
Check if argument has been supplied.
The BlackBoard abstract class.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
ValueIterator * search(const char *path)
Iterator with search results.
Laser720Interface Fawkes BlackBoard Interface.
BlackBoard interface listener.
Remote configuration via Fawkes net.
long int parse_item_int(unsigned int index)
Parse item as integer.
virtual void erase_default(const char *path)
Erase the given default value from the configuration.
virtual void close(Interface *interface)=0
Close interface.