Fawkes API  Fawkes Development Version
pitch_calibration.cpp
1 /***************************************************************************
2  * pitch_calibration.cpp - Calibrate pitch transform of the back laser
3  *
4  * Created: Tue 18 Jul 2017 16:51:36 CEST 16:51
5  * Copyright 2017-2018 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "pitch_calibration.h"
22 
23 #include <config/netconf.h>
24 
25 using namespace fawkes;
26 using namespace std;
27 
28 /** @class PitchCalibration "pitch_calibration.h"
29  * Calibrate the pitch angle of the laser.
30  * Compute the angle by computing the mean z value of all points in the rear of
31  * the robot and update the angle accordingly.
32  * @author Till Hofmann
33  */
34 
35 /** Constructor.
36  * @param laser The laser interface to fetch data from
37  * @param tf_transformer The transformer to use to compute transforms
38  * @param config The network config to read from and write updates to
39  * @param config_path The config path to read from and write updates to
40  */
42  tf::Transformer * tf_transformer,
43  NetworkConfiguration *config,
44  string config_path)
45 : LaserCalibration(laser, tf_transformer, config, config_path)
46 {
47 }
48 
49 /** The actual calibration.
50  * Apply the method continuously until the mean z reaches the threshold.
51  * Write the upated pitch angle to the config in each iteration.
52  */
53 void
55 {
56  printf("Starting pitch angle calibration.\n");
57  float mean_z;
58  do {
59  laser_->read();
60  PointCloudPtr cloud = laser_to_pointcloud(*laser_);
61  transform_pointcloud("base_link", cloud);
62  PointCloudPtr rear_cloud = filter_cloud_in_rear(cloud);
63  printf("Rear cloud has %zu points.\n", rear_cloud->points.size());
64  try {
65  mean_z = get_mean_z(rear_cloud);
66  } catch (InsufficientDataException &e) {
67  printf("Insufficient data: %s\n", e.what_no_backtrace());
68  usleep(sleep_time_);
69  continue;
70  }
71  printf("Mean z is %f.\n", mean_z);
72  float old_pitch = config_->get_float(config_path_.c_str());
73  float new_pitch = get_new_pitch(mean_z, old_pitch);
74  printf("Updating pitch from %f to %f.\n", old_pitch, new_pitch);
75  config_->set_float(config_path_.c_str(), new_pitch);
76  usleep(sleep_time_);
77  } while (abs(mean_z) > threshold);
78  printf("Pitch calibration finished.\n");
79 }
80 
81 /** Compute the new pitch based on the old pitch and the mean z.
82  * @param z The mean z value of all points in the rear of the robot.
83  * @param old_pitch The pitch that was configured when recording the mean z.
84  * @return The new pitch angle.
85  */
86 float
87 PitchCalibration::get_new_pitch(float z, float old_pitch)
88 {
89  // Note: We could also compute a more accurate new value using the measured
90  // distance and height, but this works well enough.
91  return old_pitch - z;
92 }
PitchCalibration::get_new_pitch
float get_new_pitch(float z, float old_pitch)
Compute the new pitch based on the old pitch and the mean z.
Definition: pitch_calibration.cpp:87
LaserCalibration::laser_to_pointcloud
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
Definition: laser_calibration.cpp:72
LaserCalibration::laser_
LaserInterface * laser_
The laser that provides the input data.
Definition: laser_calibration.h:89
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
fawkes::tf::Transformer
Definition: transformer.h:74
LaserCalibration::config_path_
const std::string config_path_
The config path to use for reading and updating config values.
Definition: laser_calibration.h:95
PitchCalibration::threshold
constexpr static float threshold
The threshold of the mean of z to stop calibration.
Definition: pitch_calibration.h:45
LaserCalibration
Definition: laser_calibration.h:66
PitchCalibration::calibrate
virtual void calibrate()
The actual calibration.
Definition: pitch_calibration.cpp:54
LaserCalibration::filter_cloud_in_rear
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
Definition: laser_calibration.cpp:113
LaserCalibration::transform_pointcloud
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
Definition: laser_calibration.cpp:94
LaserCalibration::config_
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
Definition: laser_calibration.h:93
LaserCalibration::get_mean_z
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
Definition: laser_calibration.cpp:132
fawkes::NetworkConfiguration
Definition: netconf.h:55
fawkes
LaserCalibration::sleep_time_
const static long sleep_time_
Time in micro seconds to sleep between iterations.
Definition: laser_calibration.h:97
fawkes::NetworkConfiguration::set_float
virtual void set_float(const char *path, float f)
Definition: netconf.cpp:731
PitchCalibration::PitchCalibration
PitchCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
Definition: pitch_calibration.cpp:41
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Laser360Interface
Definition: Laser360Interface.h:39
InsufficientDataException
Exception that is thrown if there are not enough laser points to do a matching.
Definition: laser_calibration.h:55
fawkes::NetworkConfiguration::get_float
virtual float get_float(const char *path)
Definition: netconf.cpp:247