21 #include "time_offset_calibration.h"
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>
64 step_(numeric_limits<float>::max())
79 map<float, float> costs;
80 float min_cost = numeric_limits<float>::max();
81 float min_offset = 0.;
83 uniform_real_distribution<float> random_float_dist(0, 1);
85 printf(
"Rotating bot with omega %f\n",
omega_);
96 printf(
"Taking snapshot (moving)\n");
100 printf(
"Stopping bot.\n");
106 printf(
"Taking snapshot (resting)\n");
107 PointCloudPtr rest_cloud;
119 printf(
"Insufficient data: %s.\nPlease move the robot.\n", e.
what_no_backtrace());
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",
130 next_offset = min_offset;
131 step_ = next_offset - current_offset;
133 min_cost = current_cost;
134 min_offset = current_offset;
136 next_offset = current_offset +
step_;
138 printf(
"Updating time offset from %f to %f (step %f), current cost %f\n",
144 current_offset = next_offset;
146 }
while (abs(
step_) > 0.0005);
147 printf(
"Setting to offset with minimal cost %f\n", min_offset);
166 pcl::PassThrough<Point> pass_x;
167 pass_x.setInputCloud(laser_cloud);
168 pass_x.setFilterFieldName(
"x");
169 pass_x.setFilterLimits(-3., 3.);
171 pass_x.filter(*x_filtered);
172 pcl::PassThrough<Point> pass_y;
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);
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);
191 pass_z.filter(*xyz_filtered);
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.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
void read()
Read from BlackBoard into local copy.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Laser360Interface Fawkes BlackBoard Interface.
TransRotMessage Fawkes BlackBoard Interface Message.
MotorInterface Fawkes BlackBoard Interface.
float omega() const
Get omega value.
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.