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 
33 using namespace fawkes;
34 using 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,
60  NetworkConfiguration *config,
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  */
75 void
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++) {
87  MotorInterface::TransRotMessage *rot_message =
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_);
98  MotorInterface::TransRotMessage *stop_message =
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  */
160 PointCloudPtr
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.
constexpr static float rotation_time_
The time to rotate.
constexpr static float omega_
The angular velocity to use to rotate.
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 const long sleep_time_
Time in micro seconds to sleep after each iteration.
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
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:472
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
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.