21#include "yaw_calibration.h"
23#include <config/netconf.h>
24#include <tf/transformer.h>
50 front_laser_(front_laser),
52 random_float_dist_(0, 1),
53 min_cost_(numeric_limits<float>::max()),
65 printf(
"Starting to calibrate yaw angle.\n");
72 printf(
"Insufficient data, please move the robot\n");
83 next_yaw = last_yaw +
step_;
89 printf(
"Insufficient data, skipping loop.\n");
92 printf(
"Updating yaw from %f to %f (step %f), last cost %f\n",
105 printf(
"Yaw calibration finished.\n");
137 static float last_cost = current_cost;
138 costs_[last_yaw] = current_cost;
139 float next_yaw = last_yaw +
step_;
140 for (
auto &cost_pair :
costs_) {
141 if (cost_pair.second < current_cost && cost_pair.first != last_yaw) {
142 float jump_probability =
static_cast<float>((current_cost - cost_pair.second)) / current_cost;
144 if (rand_01 > 1 - jump_probability) {
145 last_cost = current_cost;
151 printf(
"Jumping to %f, cost %f -> %f (probability was %f)\n",
156 return cost_pair.first;
160 if (current_cost > last_cost) {
163 last_cost = current_cost;
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.
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
PointCloudPtr filter_center_cloud(PointCloudPtr input)
Remove the center of a pointcloud This removes all points around the origin of the pointcloud.
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.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
float get_new_yaw(float current_cost, float last_yaw)
Compute the new yaw.
std::map< float, float > costs_
A map of yaw config values to costs.
float min_cost_yaw_
A yaw configuration with the minimal cost.
float step_
The current step size.
std::mt19937 random_generator_
Random number generator used to compute the random reset probability.
const float init_step_
The initial step size.
std::uniform_real_distribution< float > random_float_dist_
The distribution used to compute the random reset probability.
float get_current_cost(float *new_yaw)
Get the cost of the current configuration.
LaserInterface * front_laser_
The laser interface used to read the front laser data from.
YawCalibration(LaserInterface *laser, LaserInterface *front_laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
float min_cost_
The minimal cost.
virtual void calibrate()
The actual calibration.
void read()
Read from BlackBoard into local copy.
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
virtual float get_float(const char *path)
Get value from configuration which is of type float.
Fawkes library namespace.