21#include "roll_calibration.h"
23#include <config/netconf.h>
24#include <pcl/common/geometry.h>
25#include <pcl/filters/filter.h>
26#include <pcl/filters/passthrough.h>
27#include <pcl/registration/icp.h>
28#include <tf/transform_listener.h>
29#include <tf/transformer.h>
62 printf(
"Starting to calibrate roll angle.\n");
73 printf(
"Left-right difference is %f.\n", lrd);
76 printf(
"Updating roll from %f to %f.\n", old_roll, new_roll);
80 printf(
"Roll calibration finished.\n");
98 error <<
"Not enough laser points on the left, got " << left_cloud->size() <<
", need "
104 error <<
"Not enough laser points on the right, got " << right_cloud->size() <<
", need "
108 printf(
"Using %zu points on the left, %zu points on the right\n",
110 right_cloud->size());
122 return old_roll + 0.5 * mean_error;
133 std::vector<int> indices;
134 input->is_dense =
false;
135 pcl::removeNaNFromPointCloud(*input, *filtered, indices);
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.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
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 filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
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.
static constexpr float threshold
The threshold of the left-right difference to stop calibration.
RollCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
float get_new_roll(float mean_error, float old_roll)
Compute a new roll angle based on the mean error and the old roll.
virtual void calibrate()
The actual calibration.
PointCloudPtr filter_calibration_cloud(PointCloudPtr input)
Filter the input cloud to be useful for roll calibration.
float get_lr_mean_diff()
Get the difference of the mean of z of the left and right pointclouds.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
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.