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);
Laser360Interface Fawkes BlackBoard Interface.
Abstract base class for laser calibration.
static constexpr float threshold
The threshold of the left-right difference to stop calibration.
static const size_t min_points
The number of points required in a pointcloud to use it as input data.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
Fawkes library namespace.
PointCloudPtr filter_calibration_cloud(PointCloudPtr input)
Filter the input cloud to be useful for roll calibration.
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.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
RollCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
void read()
Read from BlackBoard into local copy.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
static const uint max_iterations_
The number of iterations to run before aborting the calibration.
const std::string config_path_
The config path to use for reading and updating config values.
float get_lr_mean_diff()
Get the difference of the mean of z of the left and right pointclouds.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
virtual void calibrate()
The actual calibration.
Exception that is thrown if there are not enough laser points to do a matching.
float get_new_roll(float mean_error, float old_roll)
Compute a new roll angle based on the mean error and the old roll.
static const long sleep_time_
Time in micro seconds to sleep between iterations.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
Remote configuration via Fawkes net.