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
25using namespace fawkes;
26using 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,
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 */
53void
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 */
86float
87PitchCalibration::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}
Exception that is thrown if there are not enough laser points to do a matching.
Abstract base class for laser calibration.
LaserInterface * laser_
The laser that provides the input data.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
const std::string config_path_
The config path to use for reading and updating config values.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
static const long sleep_time_
Time in micro seconds to sleep between iterations.
static constexpr float threshold
The threshold of the mean of z to stop calibration.
PitchCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
virtual void calibrate()
The actual calibration.
float get_new_pitch(float z, float old_pitch)
Compute the new pitch based on the old pitch and the mean z.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Definition: netconf.h:50
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
Definition: netconf.cpp:731
virtual float get_float(const char *path)
Get value from configuration which is of type float.
Definition: netconf.cpp:247
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.