Fawkes API Fawkes Development Version
time_offset_calibration.cpp
1/***************************************************************************
2 * time_offset_calibration.cpp - Laser time offset calibration
3 *
4 * Created: Tue 18 Jul 2017 17:40:16 CEST 17:40
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 "time_offset_calibration.h"
22
23#include <config/netconf.h>
24#include <interfaces/MotorInterface.h>
25#include <pcl/common/geometry.h>
26#include <pcl/filters/filter.h>
27#include <pcl/filters/passthrough.h>
28#include <pcl/registration/icp.h>
29#include <tf/transformer.h>
30
31#include <random>
32
33using namespace fawkes;
34using namespace std;
35
36/** @class TimeOffsetCalibration "time_offset_calibration.h"
37 * Calibrate the time offset of a laser. This is done as follows:
38 * 1. Move the robot to a place with some recognizable object in the laser,
39 * e.g., a corner
40 * 2. Start rotating the robot
41 * 3. Record a reference pointcloud
42 * 4. Stop rotating
43 * 5. Record a second pointcloud
44 * 6. Compare the two pointclouds and update the time offset based on the
45 * angle between the two pointclouds.
46 *
47 * @author Till Hofmann
48 */
49
50/** Constructor.
51 * @param laser The laser to get the data from
52 * @param motor The MotorInterface used to control the rotation of the robot
53 * @param tf_transformer The transformer to use to compute transforms
54 * @param config The network config to read from and write the time offset to
55 * @param config_path The config path to read from and write the time offset to
56 */
58 MotorInterface * motor,
59 tf::Transformer * tf_transformer,
61 string config_path)
62: LaserCalibration(laser, tf_transformer, config, config_path),
63 motor_(motor),
64 step_(numeric_limits<float>::max())
65{
66}
67
68/** Calibrate the time offset.
69 * Continuously execute the calibration procedure until the offset is small
70 * enough. To improve convergence rate, in each iteration, jump to the minimum
71 * with a certain probability based on the current cost and the minimal cost.
72 * The time offset is written to the config in each iteration. At the end, the
73 * time offset is always set to the offset with minimal cost.
74 */
75void
77{
78 float current_offset = config_->get_float(config_path_.c_str());
79 map<float, float> costs;
80 float min_cost = numeric_limits<float>::max();
81 float min_offset = 0.;
82 mt19937 random_gen;
83 uniform_real_distribution<float> random_float_dist(0, 1);
84 do {
85 printf("Rotating bot with omega %f\n", omega_);
86 for (uint i = 0; i < rotation_time_ * frequency_; i++) {
89 motor_->msgq_enqueue(rot_message);
90 usleep(1e6 / frequency_);
91 motor_->read();
92 if (motor_->omega() < 0.8 * omega_) {
93 i = 0;
94 }
95 }
96 printf("Taking snapshot (moving)\n");
97 PointCloudPtr moving_cloud = get_lasercloud(laser_);
99 new MotorInterface::TransRotMessage(0.f, 0.f, 0.f);
100 printf("Stopping bot.\n");
101 motor_->msgq_enqueue(stop_message);
102 while (motor_->omega() > 0.02) {
103 motor_->read();
104 }
105 usleep(50000);
106 printf("Taking snapshot (resting)\n");
107 PointCloudPtr rest_cloud;
108 try {
109 rest_cloud = get_lasercloud(laser_);
110 } catch (Exception &e) {
111 printf("Cloud not get pointcloud: %s\n", e.what_no_backtrace());
112 continue;
113 }
114 float yaw;
115 float current_cost;
116 try {
117 current_cost = get_matching_cost(rest_cloud, moving_cloud, &yaw);
118 } catch (InsufficientDataException &e) {
119 printf("Insufficient data: %s.\nPlease move the robot.\n", e.what_no_backtrace());
120 continue;
121 }
122 float next_offset;
123 float jump_probability = static_cast<float>((current_cost - min_cost)) / current_cost;
124 float rand_01 = random_float_dist(random_gen);
125 if (current_cost > min_cost && rand_01 > 1 - jump_probability) {
126 printf("Setting back to minimum: %f -> %f (probability %f)\n",
127 current_offset,
128 min_offset,
129 jump_probability);
130 next_offset = min_offset;
131 step_ = next_offset - current_offset;
132 } else {
133 min_cost = current_cost;
134 min_offset = current_offset;
135 step_ = -0.05 * yaw / omega_;
136 next_offset = current_offset + step_;
137 }
138 printf("Updating time offset from %f to %f (step %f), current cost %f\n",
139 current_offset,
140 next_offset,
141 step_,
142 current_cost);
143 config_->set_float(config_path_.c_str(), next_offset);
144 current_offset = next_offset;
145 usleep(sleep_time_);
146 } while (abs(step_) > 0.0005);
147 printf("Setting to offset with minimal cost %f\n", min_offset);
148 config_->set_float(config_path_.c_str(), min_offset);
149}
150
151/** Prepare the laser data for calibration.
152 * Convert the laser data into a pointcloud and filter it so it only contains
153 * data that is useful for calibration. In particular, restrict the data in x
154 * and y directions to the interval [-3,3], remove any points close to the
155 * robot in y direction, and limit the data in z direction to points above the
156 * ground and < 1m.
157 * @param laser The laser interface to read the unfiltered data from
158 * @return A filtered pointcloud
159 */
160PointCloudPtr
162{
163 laser->read();
164 PointCloudPtr laser_cloud = laser_to_pointcloud(*laser);
165 transform_pointcloud("odom", laser_cloud);
166 pcl::PassThrough<Point> pass_x;
167 pass_x.setInputCloud(laser_cloud);
168 pass_x.setFilterFieldName("x");
169 pass_x.setFilterLimits(-3., 3.);
170 PointCloudPtr x_filtered(new PointCloud());
171 pass_x.filter(*x_filtered);
172 pcl::PassThrough<Point> pass_y;
173 ;
174 pass_y.setInputCloud(x_filtered);
175 pass_y.setFilterFieldName("y");
176 pass_y.setFilterLimitsNegative(true);
177 pass_y.setFilterLimits(-0.3, 0.3);
178 PointCloudPtr xy_filtered_inner(new PointCloud());
179 pass_y.filter(*xy_filtered_inner);
180 pcl::PassThrough<Point> pass_y_outer;
181 pass_y_outer.setInputCloud(xy_filtered_inner);
182 pass_y_outer.setFilterFieldName("y");
183 pass_y_outer.setFilterLimits(-3, 3);
184 PointCloudPtr xy_filtered(new PointCloud());
185 pass_y_outer.filter(*xy_filtered);
186 pcl::PassThrough<Point> pass_z;
187 pass_z.setInputCloud(xy_filtered);
188 pass_z.setFilterFieldName("z");
189 pass_z.setFilterLimits(0.1, 1);
190 PointCloudPtr xyz_filtered(new PointCloud());
191 pass_z.filter(*xyz_filtered);
192 return xyz_filtered;
193}
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.
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.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
fawkes::MotorInterface * motor_
The motor interface used to control the rotation of the robot.
TimeOffsetCalibration(LaserInterface *laser, fawkes::MotorInterface *motor, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
PointCloudPtr get_lasercloud(LaserInterface *laser)
Prepare the laser data for calibration.
static constexpr float omega_
The angular velocity to use to rotate.
static const long sleep_time_
Time in micro seconds to sleep after each iteration.
static constexpr float rotation_time_
The time to rotate.
float step_
The current step size for the time offset.
static const unsigned int frequency_
The frequency for motor commands.
virtual void calibrate()
Calibrate the time offset.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
Laser360Interface Fawkes BlackBoard Interface.
TransRotMessage Fawkes BlackBoard Interface Message.
MotorInterface Fawkes BlackBoard Interface.
float omega() const
Get omega value.
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.