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);
Laser360Interface Fawkes BlackBoard Interface.
Abstract base class for laser calibration.
float step_
The current step size for the time offset.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
TransRotMessage Fawkes BlackBoard Interface Message.
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
virtual void calibrate()
Calibrate the time offset.
Fawkes library namespace.
float get_matching_cost(PointCloudPtr cloud1, PointCloudPtr cloud2, float *rot_yaw)
Compare two pointclouds with ICP.
LaserInterface * laser_
The laser that provides the input data.
virtual float get_float(const char *path)
Get value from configuration which is of type float.
static constexpr float omega_
The angular velocity to use to rotate.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
float omega() const
Get omega value.
static constexpr float rotation_time_
The time to rotate.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
static const unsigned int frequency_
The frequency for motor commands.
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.
const std::string config_path_
The config path to use for reading and updating config values.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
static const long sleep_time_
Time in micro seconds to sleep after each iteration.
MotorInterface Fawkes BlackBoard Interface.
Exception that is thrown if there are not enough laser points to do a matching.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr get_lasercloud(LaserInterface *laser)
Prepare the laser data for calibration.
Remote configuration via Fawkes net.