23 #include <blackboard/interface_listener.h> 24 #include <blackboard/remote.h> 25 #include <config/netconf.h> 26 #include <core/threading/thread.h> 27 #include <core/threading/wait_condition.h> 28 #include <interfaces/Laser360Interface.h> 29 #include <interfaces/Laser720Interface.h> 30 #include <netcomm/fawkes/client.h> 31 #include <utils/system/argparser.h> 32 #include <utils/time/time.h> 43 #define MAX_WAIT_TIME 60 44 #define DEFAULT_WAIT_TIME 10 45 #define DEFAULT_NUM_MEASUREMENTS 100 46 #define DEFAULT_COMPARE_DISTANCE 0.9 48 #define INITIAL_MEASUREMENT 123456.0 53 print_usage(
const char *program_name)
55 printf(
"Usage: %s [-h] [-r host[:port]] <num_spots> <config_prefix>\n" 56 " -h This help message\n" 57 " -r host[:port] Remote host (and optionally port) to connect to\n" 58 " -n <NUM> Number of measurements to use, defaults to %d\n" 59 " -w <SEC> Wait time in seconds, defaults to %d\n" 60 " -c <DIST> Compare distance in m, defaults to %f\n" 61 " -m <MARGIN_DEG> Margin in degree to add around dead spot regions\n" 62 " -d Dry-run, do not save results to configuration\n" 63 " -b Show data by opening a blackboard laser interface\n" 64 " -i <ID> Open laser interface named <ID>\n" 65 "<num_spots> Expected number of dead spots\n",
67 DEFAULT_NUM_MEASUREMENTS,
69 DEFAULT_COMPARE_DISTANCE);
92 unsigned int num_measurements,
93 float compare_distance,
100 laser720_ = laser720;
101 laser360_ = laser360;
102 blackboard_ = blackboard;
103 num_spots_expected_ = num_spots;
104 num_measurements_ = num_measurements;
105 cur_measurement_ = 0;
108 compare_distance_ = compare_distance;
109 measurements_.clear();
110 num_spots_found_ = 0;
112 if (!laser720_ || !laser720_->has_writer()) {
113 lowres_calibrate_ =
true;
114 num_beams_ = laser360_->maxlenof_distances();
115 bbil_add_data_interface(laser360_);
117 lowres_calibrate_ =
false;
118 num_beams_ = laser720_->maxlenof_distances();
119 bbil_add_data_interface(laser720_);
121 std::vector<float> tmp;
122 tmp.resize(num_measurements_, INITIAL_MEASUREMENT);
123 measurements_.resize(num_beams_, tmp);
125 blackboard_->register_listener(
this);
132 start_measuring_ =
true;
133 finish_waitcond_.wait();
139 std::vector<std::pair<float, float>>
151 return num_spots_found_;
156 calculate_median(std::vector<float> measurements)
158 std::sort(measurements.begin(), measurements.end());
159 return measurements[measurements.size() / 2];
165 std::vector<float> rv;
166 rv.resize(num_beams_, INITIAL_MEASUREMENT);
168 for (
unsigned int i = 0; i < measurements_.size(); ++i) {
169 rv[i] = calculate_median(measurements_[i]);
179 float angle_factor = 360.0 / num_beams_;
181 std::vector<float> medians = calculate_medians();
183 bool iteration_done =
false;
184 for (
unsigned int i = 0; !iteration_done && i < medians.size(); ++i) {
185 if (medians[i] == INITIAL_MEASUREMENT) {
186 printf(
"WARNING: No valid measurement at angle %f°!\n", i * angle_factor);
190 if (medians[i] < compare_distance_) {
192 float start_angle = i * angle_factor;
199 if ((i + 1) >= medians.size()) {
200 if (iteration_done) {
201 printf(
"Could not find end for region starting at %f°, all values " 206 iteration_done =
true;
212 }
while ((medians[i] < compare_distance_) && (medians[i] != INITIAL_MEASUREMENT));
213 if (medians[i] >= compare_distance_) {
214 float end_angle = i * angle_factor;
216 dead_spots_.push_back(std::make_pair(start_angle, end_angle));
228 std::sort(dead_spots_.begin(), dead_spots_.end());
232 merge_region(
unsigned int ind1,
unsigned int ind2)
234 if (dead_spots_[ind1].second >= dead_spots_[ind2].first) {
236 if (dead_spots_[ind1].first > dead_spots_[ind2].second) {
243 dead_spots_[ind1].second = 360.;
244 dead_spots_[ind2].first = 0.;
250 dead_spots_[ind1].second = dead_spots_[ind2].second;
251 dead_spots_.erase(dead_spots_.begin() + ind2);
263 while (i < dead_spots_.size() - 1) {
266 if (merge_region(i, i + 1))
270 unsigned int last = dead_spots_.size() - 1;
271 if ((dead_spots_[last].second >= dead_spots_[0].first)
272 && (dead_spots_[last].second <= dead_spots_[0].second)
273 && (dead_spots_[0].first >= dead_spots_[last].first - 360)
274 && (dead_spots_[0].second <= dead_spots_[last].second)) {
275 merge_region(last, 0);
283 if (margin_ != 0.0) {
286 for (
unsigned int i = 0; i != dead_spots_.size(); ++i) {
289 dead_spots_[i].first -= margin_;
290 dead_spots_[i].second += margin_;
291 if (dead_spots_[i].second > 360.0) {
292 dead_spots_[i].second -= 360.0;
308 for (
unsigned int i = 0; i != dead_spots_.size(); ++i) {
309 if (dead_spots_[i].first < 0.) {
312 dead_spots_[i].first = 360. + dead_spots_[i].first;
314 if (dead_spots_[i].second < 0.) {
317 dead_spots_[i].second = 360. + dead_spots_[i].first;
320 if (dead_spots_[i].first > dead_spots_[i].second) {
324 dead_spots_.resize(dead_spots_.size() + 1);
325 for (
int j = dead_spots_.size() - 1; j >= (int)i; --j) {
326 dead_spots_[j + 1] = dead_spots_[j];
328 dead_spots_[i + 1].first = 0;
329 dead_spots_[i].second = 360.0;
343 for (
unsigned int i = 0; i != dead_spots_.size(); ++i) {
344 printf(
"Spot %u start: %3.2f end: %3.2f\n",
346 dead_spots_[i].first,
347 dead_spots_[i].second);
352 process_measurements()
356 if (dead_spots_.size() > 0) {
360 num_spots_found_ = dead_spots_.size();
363 num_spots_found_ = 0;
366 if (num_spots_found_ != num_spots_expected_) {
367 printf(
"Error, expected %u dead spots, but detected %u.\n",
372 printf(
"Found expected number of %u dead spots\n", num_spots_expected_);
373 if (dead_spots_.size() > num_spots_expected_) {
374 printf(
"Note that more regions will be printed than spots were expected.\n" 375 "This is due to splitting that occurs around the discontinuity at 0°/360°\n");
377 if (num_spots_expected_ > dead_spots_.size()) {
378 printf(
"Note that less regions will be printed than spots were expected.\n" 379 "This is due to merging that occurred after applying the margin you\n" 380 "suggested and normalizing the data.\n");
387 bb_interface_data_changed(
Interface *interface)
throw()
389 if (!start_measuring_)
392 printf(
"\r%3u samples remaining...", num_measurements_ - cur_measurement_);
395 float * distances = NULL;
396 unsigned int num_distances = 0;
397 if (lowres_calibrate_) {
399 distances = laser360_->distances();
400 num_distances = laser360_->maxlenof_distances();
403 distances = laser720_->distances();
404 num_distances = laser720_->maxlenof_distances();
407 for (
unsigned int i = 0; i < num_distances; ++i) {
408 if (finite(distances[i]) && distances[i] > 1e-6) {
409 measurements_[i][cur_measurement_] = distances[i];
413 if (++cur_measurement_ >= num_measurements_) {
414 printf(
"\rMeasuring done, post-processing data now.\n");
415 process_measurements();
416 blackboard_->unregister_listener(
this);
417 finish_waitcond_.wake_all();
428 bool lowres_calibrate_;
429 bool start_measuring_;
430 unsigned int num_spots_expected_;
431 unsigned int num_beams_;
432 unsigned int num_measurements_;
433 unsigned int cur_measurement_;
434 unsigned int num_spots_found_;
435 float compare_distance_;
436 std::vector<std::vector<float>> measurements_;
437 std::vector<std::pair<float, float>> dead_spots_;
441 main(
int argc,
char **argv)
445 if (argp.has_arg(
"h")) {
446 print_usage(argv[0]);
450 char * host = (
char *)
"localhost";
451 unsigned short int port = FAWKES_TCP_PORT;
452 long int num_measurements = DEFAULT_NUM_MEASUREMENTS;
453 long int wait_time = DEFAULT_WAIT_TIME;
454 float compare_distance = DEFAULT_COMPARE_DISTANCE;
456 std::string interface_id =
"Laser";
457 std::string cfg_prefix =
"";
459 if (argp.has_arg(
"n")) {
460 num_measurements = argp.parse_int(
"n");
461 if (num_measurements <= 0) {
462 printf(
"Invalid number of measurements, must be > 0\n\n");
463 print_usage(argp.program_name());
467 if (argp.has_arg(
"w")) {
468 wait_time = argp.parse_int(
"w");
470 printf(
"Invalid wait time, must be integer > 0\n\n");
471 print_usage(argp.program_name());
473 }
else if (wait_time > MAX_WAIT_TIME) {
474 printf(
"Wait time of more than %d seconds are nonsense, aborting.\n\n", MAX_WAIT_TIME);
475 print_usage(argp.program_name());
479 if (argp.has_arg(
"c")) {
480 compare_distance = argp.parse_float(
"c");
481 if (compare_distance < 0) {
482 printf(
"Invalid compare distance, must be > 0\n\n");
483 print_usage(argp.program_name());
487 if (argp.has_arg(
"m")) {
488 margin = argp.parse_int(
"m");
489 if ((margin <= -360) || (margin >= 360)) {
490 printf(
"Invalid margin, must be in the ragen [-359, 359]\n\n");
491 print_usage(argp.program_name());
495 if (argp.num_items() == 0) {
496 printf(
"Number of expected dead spots not supplied\n\n");
497 print_usage(argp.program_name());
499 }
else if ((argp.num_items() == 1) && !argp.has_arg(
"d")) {
500 printf(
"Config prefix not given and not dry-run\n\n");
501 print_usage(argp.program_name());
503 }
else if (argp.num_items() > 2) {
504 printf(
"Too many arguments\n\n");
505 print_usage(argp.program_name());
507 }
else if (argp.num_items() == 2) {
508 cfg_prefix = argp.items()[1];
509 if (cfg_prefix[cfg_prefix.length() - 1] !=
'/') {
514 if (argp.has_arg(
"i")) {
515 interface_id = argp.arg(
"i");
517 bool free_host = argp.parse_hostport(
"r", &host, &port);
529 printf(
"Failed to connect to remote host at %s:%u\n\n", host, port);
543 printf(
"Failed to open blackboard interfaces");
549 printf(
"No writer for neither high nor low resolution laser.\n" 550 "Laser plugin not loaded?\n\n");
551 blackboard->
close(laser360);
552 blackboard->
close(laser720);
557 printf(
"Warning: high resolution laser not found calibrating with 1° resolution.\n" 558 " It is recommended to enable the high resolution mode for\n" 559 " calibration. Acquired 1° data may be unsuitable when used in\n" 560 " high resolution mode!\n\n");
561 blackboard->
close(laser720);
569 printf(
"Position the laser such that it has %f m of free space around it.\n\n" 570 "Also verify that the laser is running with disable filters\n\n",
574 while ((diff = (now - &start)) < wait_time) {
575 printf(
"\rCalibration will start in %2li sec (Ctrl-C to abort)...",
576 wait_time - (
unsigned int)floor(diff));
581 printf(
"\rCalibration starting now. \n");
583 unsigned int num_spots = argp.parse_item_int(0);
587 num_spots, num_measurements, compare_distance, margin, blackboard, laser360, laser720);
590 std::vector<std::pair<float, float>> dead_spots = calib->
get_dead_spots();
592 if (!argp.has_arg(
"d")) {
594 printf(
"Number of spots does not match expectation. Not writing to config file.");
596 printf(
"Storing information in remote config\n");
600 for (
unsigned int i = 0; i < 2; ++i) {
604 while (vit->
next()) {
615 for (
unsigned int i = 0; i < dead_spots.size(); ++i) {
617 if (asprintf(&prefix,
"%s%u/", cfg_prefix.c_str(), i) == -1) {
618 printf(
"Failed to store dead spot %u, out of memory\n", i);
621 std::string start_path = std::string(prefix) +
"start";
622 std::string end_path = std::string(prefix) +
"end";
624 netconf->
set_float(start_path.c_str(), dead_spots[i].first);
625 netconf->
set_float(end_path.c_str(), dead_spots[i].second);
632 blackboard->
close(laser360);
633 blackboard->
close(laser720);
635 if (argp.has_arg(
"b")) {
638 for (
unsigned int i = 0; i < 720; ++i) {
641 for (
unsigned int i = 0; i != dead_spots.size(); ++i) {
642 const unsigned int start = (
unsigned int)dead_spots[i].first * 2;
643 unsigned int end = (
unsigned int)dead_spots[i].second * 2;
648 for (
unsigned int j = start; j <= end; ++j) {
653 printf(
"Storing data in BlackBoard for visualization. Press Ctrl-C to quit.\n");
Laser360Interface Fawkes BlackBoard Interface.
Wait until a given condition holds.
Simple Fawkes network client.
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.
void wait_finished()
Wait for the calibration to be finished.
Calibrator for dead ranges.
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.
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.
virtual void erase_default(const char *path)
Erase the given default value from the configuration.
virtual void close(Interface *interface)=0
Close interface.